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I. INTRODUCTION 


The problems of torpedo tracking have been studied, at 
the Naval Postgraduate School, by several researchers, who 
have implemented simulations that would allow analysis of 
tracking algorithm performance using the Extended Kalman 
Filter [1]. However, those studies have been limited to 
Simulations done on large mainframe computers, without 
trying to adapt them to the real time processing system at 
the torpedo range. 

The objective here is to develop a version of the 
Extended Kalman Filter, adapted to the constraints of 
real-time processing, by generating a program in the C 
language. Also it is designed to run in the microcomputer 
C.I.E 680/30, currently utilized at the Naval Torpedo 
Station, Washington. 

The presentation herein tries to Folios as much as 
possible the chronological order, describing the problems in 


the order of appearance and the way they were solved. 


II. PROBLEM DEFINITION 


A. RANGE TRACKING GEOMETRY 
Sets of hydrophone arrays are distributed in line along 
the extension of the test range. Each individual array 
consists of four independent elements, placed at a distance 
"d" apart, on the vertices of a cube, thus defining an 
orthogonal coordinate system in which the measurement of the 
transit times are made. Figure 1, shows the geometry. 
T 


Four transit times, T ۲ "و‎ ۷ 


ci "x corresponding to the 


۷٣ z’ 
four hydrophones in the array are measured and the 
positional information is computed from the measured transit 
times. The torpedos are equipped with sonar transmitters 
which are synchronized with a master clock, allowing new 
data every 1.31 seconds, with a range accuracy between 3 to 
50 feet. 

When multiple arrays are used, the signal from the 
Closest hydrophone is defined as the basis for the time 
measurements and for the range calculations. If the computer 


time were not critical, an 18 hydrophone signal should be 


processed. 


(x,y, z) 
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Figure 1: Geometry of a tracking array. 
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B. GENERAL EXTENDED KALMAN FILTER EQUATIONS 

A summary of the filter and associated equations is 
given in this section, following the order and the 
conventions in ۰. 

1. Observation Equations 

For the three-dimensional problem, in which the 

z-position is considered invariant, a state space consisting 
of the three positions, X, Y and Z, and the two velocities, 


Y and T define the torpedo motion. Thus, the discrete 


linear estimation and the nonlinear observation equations 
are given by 

X(k+1) = # . X(k) + p . W(k) (2.1) 

Z(k) = h(X(k),k) + V(k) (2.2) 
where: $ and p are constant matrices; 

h is a nonlinear function of the state X; 

W(k) is the plant excitation noise; 

V(k) is the measurement noise. 

The plant noise and the measurement noise are 
assumed Gaussian distributed with zero mean, and 
uncorrelated. 


The last assumption implies that 


G' CO S‏ 5 رر اس رم یع 


ELV(k).V| (j)1 = R(k) s. 
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where: § = 1, for k= j; 
§ = وه‎ for k aj s 
2. Linearized Equations 
Linearization of the observation equation is 
obtained by using the Taylor Series to expand about the 
prediction of the trajectory for the interval. 


Z(k) 9» H(kO . X(k) + رم‎ (2, Sa) 


where: H(k) = (2. 3b) 


gh ^ 
3X | X(k) — X(k/k— 1) 


M^ 
X(k/k-1) is the estimated value (predicted) at time 
k, given the measurements until time k-1. 


S: State Error Vector 


X(k) — X(k) — X(k) 


4. Predicted State Error Vector 


X(k/k—1) — X(k/k—1) — X(k) 
S. Covariance of State Error 


Pik) = ELX(k/k) «X) (k/k) J 


6. Predicted Covariance 
P(k/k-1) 2 EEX(k/k-1).X (k/k-1)3 


7 Stata Excitation Matrix 


Q(k) = p(k) ELW(k).W! (ke) dep! ck) 


8. Measurement Noise Covariance Matrix 


و رم آپے R(k) = ELV(k)‏ 
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Kalman Filter Equations 


P(k+1/k)=#.P(k/k).# + Q(k) (2, 4a) 
GO SP Gok-12H! Go EHGO P Gc/k-) B! ao «R Go 2171 (2:46) 
P(OSEI-80O.HGO 1: Pk/ k-1) (2. 4c) 
X(k+1/k)25. X(k/k) (2. 4d) 
Z(k/k-1)=h(X(k/k-1),k) (2. 4e) 
X (k/k) SX k/k-1) *BUO EZOO - 2 (Oc/ k- 1) 2 (2.44) 


C. APPLIED EQUATIONS L11 


In 


order to apply the Extended Kalman filter to the 


torpedo tracking problem it is necessary to define the state 


vector and the matrices H, $, p, W, B, and R. 


1. 


wheres 


Transit Times 


Th (kd Soar (x4d/2)? + (ytd /2)2 « (24/2)? 2* ^? ev ck) (2.5) 


T, (K) egit (7d/2)* « (y«d/2)? e (2«d/2)* 3 2 +v (k) (2. b) 


T (ered yao rd72)2 1^ 42 sv (k) (217) 
y VEL 


T, (KO =yÊ[ («+d /2)2 + (y+d/2)? + (z-d/2) 1 ^? ev (k) (2.8) 


d=30 feet, is the distance between two Aydrophones 
in a given arrays; 

VEL=4860 ft/sec, is the average sound speed at Dabob 
Bay. 


These equations must be linearized to apply Kalman 


Filter Theory. The linearization is done, in this case, 
about the predicted trajectory, which is being generated by 
the filter algorithm. 

The values of transit times are corrupted by noise, 
vik), which is) assumed Gaussian with zero mean. “The 
positions x, y, and 2 are all functions of the time 
interval. 


Da State Equations 


The plant state equations are defined by 


x (k+1) X (kI+TV (k) tg, 
V.o (k+1) V, (k)+g, 
y (k+1) = W r s (2.9) 
V (k+1) V (k)+ 
y vh du 
sk +1.) z (k) +g, 


where: x(k), yk) and 2(k) are the position coordinates at 


time t(k); 


V, CO and V are the x and y components of the 


velocity; 

the depth z(k) is maintained constant and any 
velocity in z is assumed random, Gaussian, 
uncorrelated and with zero mean; 

۲ is the sampling interval Tz1.31sec; 


g, > 9,5 9, and g, are excitation terms, included to 


3 


take into account random variations in speed (YO 2) 3 
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heading (Yo ), and depth ٢6 which are assumed to 
t 


be independent, zero mean, rates of change. 


Typical torpedo maneuvering parameters are 


54 = 22°/sec; Se¢= ELy,2 3 

S. Ve Ve 
= 2. gu 2 

"o. 36 ft/sec*; "b. ELY, J 
= s 2 = 

s, 7 1 ft/sec; 2 Ely. 3 


In state form 4: 


X(k+1) = #.X(k) + p.W(k) (2.10) 
1 اد‎ O aaO 
O 1 O O O 
a > O O 1 T O 
O O O 1 O 
O O O O 1 
۲/2 O O 
T 0 0 

n = | o T?/2 O (211) 
0 T 0 
0 0 m 

O 2 2. 2 ^ 
oy وٹ‎ zo. EIS (2.12a) 
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wheres 


2 2 2 
Gas = (V ۷ )g + ليا‎ 8 
= 2 
o Vx y 99, ^ Vt 5 دج‎ 


Q is a symmetric matrix given by 


0 qi q2 03 q4 
5 qé q7 q8 q? 
Q = | q10 qil qi2 q13 14 
15 qió q17 18 q1? 
20 021 022 253 4 
qos (T? /2) s.t; q1- (7 /2) s, $5 
q3- (T! /2) sk - os. و‎ 94-0: 
= 3 © = T a 
67 (T E 08-71 O 
qi2=(T?/2)2 5.2; qi3=(T? /2) 5.23 
y ۷ 
۹18-27۶ وم‎ 19-0 
2 )۷( = CT_¢(k) T (k) T (k) T (k)] 
c x y z 
or Q 0 0 
c 
2 
0 6T Q 0 
A 
R(k) = 0 0 sr 0 
۷ 
0 2 
0 0 = 


16 


(2.12b) 
(2.12c) 
(2.15) 
-(T? 2 5 
q2-(T* /2) PEE 
شا‎ 5 
q9-0; 
qi1420; 


424-7? 5,2: 


T 


where: 


and 


Typically sr =s L =° 
c X 


ho hl 

h5 hó 
H(k) = ۲ 1 0 hil 

his hiá 

h20 h21 
hO=(x' (k)+d/2)/DEN1; 
h2-(y' (k)*d/2) /DEN1; 
۲۱4< )2 ' (k) *d/2) /DEN1; 
hé=03 
he=0; 
h10=(x ‘ (k)+d/2) /DENS; 
h122(y'(k)—d/2) /DENS3; 
h142(z'(k)-*d/2)/DEN3; 
h1620; 


h1820; 

DEN1-E(Gc' (O *d/2)? (y 
DEN2-[ (x ' (k)-d/2)? *(y 
DENS=E (x ‘ (k) +d/2)? - (y 


DEN4=[ (x (k) +d/ 2) +(y 


+ - د‎ = 107^ * sec. 


h2 h3 h4 

h7 h8 h9 

h12 his h14 

h17 h18 h19 

h22 h23 h24 
h120; 
h320s 
hoz(« (k)-d/ 2) /DEN2; 
h7=(y' (k)+d/2) /DEN2; 
h9=(2 ° (k)*d/2) /DEN2; 
h11=0; 
h1320; 
hiS=(x’ (k) +d/2) /DEN4; 
hi7=(y’ (k)+d/2) /DEN4; 
h192(z'(k)—d/2)/DEN4; 


'(k)+d/2)2 +(z ' (k) +d/2)2 11 2 


'(k)+d/2)2 +(z ' (k) +d/2)2 3° ^? 


'(k)—d/2)2+(z ° (k)y+d/2)2 3! ^? 


TO رد‎ oS 
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D. THE SEQUENTIAL EXTENDED KALMAN FILTER APPROACH [11 

This approach assumes that all four transit times 
measurements are statistically independent of each other. In 
this way one can process the transit times sequentially and 
independently. The result of each computation step is used 
immediately, as soon as it is obtained. Thus more accurate 
estimates of the filter states are obtained. 

This procedure also circumvents the matrix inversion 
when determining the filter gain, and allows one to 
disregard any particular time measurement that was found 
invalid, and not the total four times for a time slot 
(T21. šlsec). 

Erroneous transit times are detected by implementing a 
three sigma gate using the covariance of the measurement 
noise (R) and the covariance of the estimation error 


(P(k/k). For i=1 to 4 , 
Qate(i) = S* C/(OP ; maximum) /4860 + را‎ » 


It can also be used to minimize position errors during 
high dynamic maneuvers, by repeating the calculations for 
the same time interval until the transit time residual error 


falls below a preset value. 
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III. THE IMPLEMENTATION PROBLEMS 


A. THE HARDWARE AND THE SOFTWARE PROBLEMS 

Since the objective is to use the developed software for 
torpedo tracking at the NAVTORPSTA, in Keyport, Washington, 
there was no other choice but to implement the program to 
run on the C.I.E. 680/30 microcomputer, and to implement it 
in the C language. 

Two problems immediately arose. 

First, there was only badly written and incomplete 
documentation. The manual was found 0ھ‎ to read and to 
understand. و‎ addition, there was no information about the 
computer hardware. 

Secondly, poor software was available. The CIE 680/30 
is a Unix based machine, designed for business applications, 
working with the Regulus operating system. As far as known, 
there is no software available (specifically written for the 
CIE) on the market. 

As aresult of these problems, there was no full-screen 
editor available for the C.I.E machine, no communications 
program, and no built in graphics capability (although the 
first screen that appears after booting the system shows the 


message "CIE GRAPHICS SYSTEM #7"). 
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B. THE FAST APPROACH 

In order to be able to write a reasonably long and 
complex program, in a reasonable time, debug it and make it 
work, it would be necessary to have a good compiler, a good 
full screen editor and good documentation on software and 
hardware. 

It was then decided to make use of the known software 
and hardware information available for the IBM-PC 
microcomputer. It was not difficult to find Public Domain 
Programs able to communicate with Unix based machines, to 
emulate the VT-100 t2rminals, and fc transfer files. Also it 
Ame not difficult to find a powerful easy to use full-screen 
editor for the IBM-PC. 

The program was then written, trying to keep the C code 
the most standard possible, using the Lattice C compiler. 
It was executed first in the IBM-Pc, then transferred 
(source code) to the C.I.E using a public domain program 
(SIMTERM). It was then adapted, making minor changes, and 


recompiled on the C.I.E to generate the executable code. 


C. ODDS AND EVENS QF C 

The C language is not very high level but it is a very 
powerful language, allowing bit operations, low level read 
and write, system calls and easy interfaces with machine 


language. The general mathematical expressions can be kept 
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the same way as in Fortran, but some new operations, 
functions and expressions are also used. 

Incrementing a variable: 

"'i=i+1;" can be "i++" or "++i", depending if you 
first use the variable (++i) and increment it or 
vice versa (itt). 

Incrementing i by j: "i-*zj;" 

Do loop, testing the counting variable after using it: 

"for (1=0si<imax3it++) ؟‎ " 

Blocks are delimited by "¢ لش‎ 

Every program, subprogram, functions or procedures are 
seen as FUNCTIONS in C, including the main program. This is 
indicated by the calling statements as 9 

main () nean for the main program that is not 
passing any arguments; 

Plot (x,y)? .... for a routine that passes X and 
Y vectors for plotting, and so on; 

C uses passing by value approach. So the value of the 
variable "x" is copied in and used inside a function. Arrays 
are equivalent to pointers and thus they can return new 
values from functions. 

There are automatic, local, global and external 
variables. There are character, integer, long integer, 
string, floating point (double and single precision), 
string, and enumeration data types. Short integers have 


standard size. Double precision floating point allows one to 
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0 و‎ 
represent numbers in the range + 101 in the IBM-PC 


(Lattice C compiler) and in the range 1038 in the CIE. 
There are IF , IF THEN ELSE, WHILE, DO .. WHILE, and CASE 
statements. 

There are no input and output statements as in Fortran 
or Other languages. Input-output is done by standard 
functions, which must be carefully used. 

Header files and ۲ #include " statements are used to 
hide from the user definitions of parameters that are 
machine dependent or contain environment-specific 
information. "#define" statements are used to define 
constants and macros and to: hide machine dependent code 
segments. 

Reference [4] is the definition manual of the language. 
The other references for the C language were used primarily 


to look over the example programs, and functions. 


IV. THE REAL-TIME KALMAN FILTER PROGRAM IN C 


A. THE INITIAL APPROACH 

The first idea was the simple translation of the program 
described in reference [1] to the C language, without 
worrying about specific problems that would arise when 
trying, later on, in a more realistic approach, to simulate 
as close as possible the actual running conditions. 

The initial program had a structure similar to the 
Fortran program in [1], that is, a main program, the actual 
Kalman Filter, and several -functions to calculate the 
trajectory, to add, transpose and multiply matrices and 
to multiply two vectors. All the floating point operations 
were done in double precision and the matrices were defined 
as two dimensional arrays. | 

The initial approach failed in many aspects. 

It did not have enough speed, producing, on the average, 
one estimate every 22 sec, in an  IBM-PC without math- 
coprocessor (8087), running at 5 MHz. The same program 
generated a new estimate every 5 sec in an IBM-FC with an 
8087 chip. It did not have the main frame graphics 
capability, readily available and, finally, due to the way 
the C language passes parameters to the subroutines, the 
matrix routines worked correctly only if matrices were 


defined as square matrices. 


B. OPTIMIZATION OF EXECUTION TIME 

Although the C.I.E microcomputer runs a Motorola 68000 
cpu at 8 MHz, and the IBM-PC runs at 5 MHz, executing the 
same program in both machines showed a difference of 104 to 
20% in execution time. In this way, optimizing the execution 
in the IBM-PC (with 8087) would be a valid approach since it 
would allow a safety margin, and would require almost the 
same effort. 

Optimization of execution time led to : 

1. Transforming all the matrices from bidimensional to 
unidimensional arrays -(or vectors). That saved memory space, 
and forced the access to the elements to conform with the 
way these are stored (C stores array elements by rows), thus 
speeding up retrieving when needed. 

2.  Eliminating functions, actually by having everything 
in the main program. 

3. Eliminating all the unnecessary operations. That 
involved restructuring the program to avoid loops when 
possible and reasonable, avoiding multiplying by zero, 
avoiding adding zeros, and avoiding repeating calculations 
when the result was known to be constant. 

4. Avoiding double precision where possible, and when 
improvement of the execution time could be achieved without 
compromising the results. The Lattice C compiler, in the 


IBM-PC, is defined for precision (all the floating point 
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calculations are done in double precision). Then, if single 
precision is desired, you lose in both, precision and speed. 
The CIE machine calculations uses the Motorola "Fast 
Floating Point", a format which is not compatible with the 
IEEE format. The IEEE floating point values lose some 
precision due to the fact that the calculations use the fast 
floating point routines anda translation to and from the 
FFP format [101]. So, the FFP format was used, in the CIE. 

5۰ Writing single precision routines to avoid calling 
standard mathematical routines, which are all double 
precision, in C. 

6& Reproducing more closely unm way the final program 
ES nave to work, when in the real time application. 

7. Testing the program for several values and 
parameters, to determine bounds that would allow results of 
good precision without increasing the execution time. It was 
found, for example, that the results are highly sensitive to 
the way the columns of the matrix H are calculated, thus, 


requiring calculation of H using double precision. 


8. Measuring execution time of tasks inside the 
program. 
9. Developing software tools to allow for interactive 


execution and checking of correctness of the results. 
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v. TESTING 


A. CHECKING THE TRAJECTORY-FOLLOWING PERFORMANCE 

The trajectory following performance was verified for 
the following trajectories : 

1. Straight line parallel to the array axis, and 
y-coordinate = 0.0 , single array tracking; 

E Straight line parallel to the array axis and 


y-coordinate = 2121.0 , single array tracking; 


3. Straight line, 452 inclination relatively to the 
array axis, single array: 

4. Straight line parallel to the array axis, long run, 
multiple array tracking; 

5. Straight line and circular maneuver, short run, slow 
turn (10.0dg/sec , 1/4-G), single array tracking; 

6. Straight line and circular maneuver, long run, slow 
turn (10.90dg/sec , 1/4-G), multiple array tracking; 

rar Straight line and circular maneuver, short run, fast 
turn  (20dg/sec  , 1-G), single array tracking, adaptive 
maneuver implemented with 4 iterations allowed: 

8. Straight line and circular maneuver, long run, fast 
turn (20 dg/sec , 1-G), multiple array tracking, adaptive 


maneuver implemented with 4 iterations allowed: 


TABLE 1 , and TABLE 2 summarize the results in terms of 
maximum absolute error for the IBM and the CIE microcom- 
puters, respectively. For the single array cadcm only the 
region inside a 3000 feet radius around the center of the 
array being used is considered in determining the errors. 


The corresponding plots can be found in the following 


pages. The plots were obtained with the value 10° for the 
elements in the diagonal of the matrix R (the Error 


Covariance Matrix) for the IBM-PC microcomputer and with the 


value 10“ for the CIE. The plots are numbered sequentially 
and contain headings to identify and relate them to the 
Table 1 and Table 2. The actual time is obtained By 
multiplying the time scale by the length of the time slot, 


1.31 seconds. 


B. CHECKING THE EXECUTION TIME REQUIREMENT 

The execution time in the IBM-PC, was verified Dy 
checking the printouts containing the values of the internal 
computer time for a cycle. Later, an "waiting loop" was 
introduced to generate an interval of 1.21 sec between each 
data input (that is, reading files TRJ.DAT and ZIC.DAT). 

Since the "reading internal time" was not incorporated 
in the CIE program version (CIEKR1), and since that proved 
to be a faster machine, the time requirement was just 


checked against a chronometer for 150 iterations. 
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Figure 14. IBM-PC - True trajectory af the torpedo in the 
area of a single array. Initial conditions : x=3000 ft, 
y22121 ft, Vx=-33.8 ft/sec, Vy=0 ft/sec. 
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Figure 15. IBM-PC - Estimate of torpedo position during a 
straight run in the area of single array. Initial conditions 
are: x=3000 ft, y=2121 ft, Vx--353.8 ft/s, MyzO ft/s. 
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Figure 16. IBM-PC - Error in torpedo position during a 
straight run in the area of a single array. Initial 
conditions: xs3000 ft, y=2121 ft, Vx3-33.8 ft/s, Vy=0 ft/s. 
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Figure 17. IBM-PC ~- Error in torpedo position during a 
straight run in the area Of a single array. Initial 
conditions: x=3000 ft, y=2121 ft, Vx=-33.8 ft/s, Vy=0 ft/s. 
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Figure 20. IBM-PC — Filter estimate of mean-square error 
during a straight run in the area of a single array. Initial 
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Figure 21. IBM-PC - Filter estimate of mean-square error 
during a straight run in the area of a single array. Initial 
conditionss x=3000 ft, y=2121 ft, Vx=-33.8 ft/s, Vy=0 ft/s. 
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Figure 22. IBM-PC - Real time plot of estimated trajectory 
of the torpedo in the area of a single array. Initial condi- 
tions: x=3000 ft, y=-3000 ft, Vx=-33.8 ft/s, Vy=-33.8 ft/s. 
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Figure 23. IBM-PC - True trajectory of the torpedo in the 
area of a single array. Initial conditions : x-35000 ft, 
y=-3000 ft, Vx=-33.8 ft/sec, Vy=35.8 ft/sec. 
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Figure 24. IBM-PC - Estimate of torpedo position during a 
straight run in the area of single array. Initial conditions 
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Figure 30. IBM-PC -~ Filter estimate of mean-square error 
for a straight run in the area of single array. Initial con- 
ditions: x=3000 ft, y=-3000 ft, Vx=-33.8 ft/s, Vy=33.8 ft/s. 
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Figure 3i. IBM-PC - True trajectory of the torpedo in the 
during a straight runthrough multiple arrays. Initial condi- 
tions: x=34000 ft, y=6050 ft, Vx=-84.4 ft/sec, Vy=0 ft/sec. 
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Figure 34. IBM-PC - Error in torpedo position during a 
straight run through multiple arrays. Initial conditions 
ares x=54000 ft, y=6050 ft, Vx=-84.4 ft/s, Vy=0 ft/s. 
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Figure 35. IBM-PC - Error in torpedo depth during a 
Straight run through multiple arrays. Initial conditions 
ares x=34000 ft, y=6050 ft, Vx=-84.4 ft/s, Vy=0 ft/s. 
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Figure 36, IBM-PC — Filter estimate Of mean-square error 
for a straight run through multiple arrays. Initial condi- 
tions: x-34000 ft, y=6050 ft, Vxz-84.4 ft/s, VyzO ft/s. 
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Figure 39. IBM-PC - Real time plot of estimated trajectory 
of the torpedo in the area of a single array. Initial condi- 
tions: x-235000 ft, yzO ft, Vx=50 ft/s, Vy=5 ft/s. 
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Figure 40.  IBM-PC - True trajectory of the torpedo during a 
10 dg/s maneuver in the area of a single array. Initial 
conditions: x-3OOO ft, yzO ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 41. IBM-PC - Estimate of torpedo position during a 
10 dg/s maneuver in the area of a single array. Initial 
conditions: x=3000 ft, y=O ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 42. IBM-PC - Error in torpedo position during a 10 
dg/s maneuver in the area of a single array. Initial 
conditions: x=3000 ft, yzO ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 43. IBM-PC - Error in torpedo position during a 10 
dg/5 maneuver in the area of a single array. Initial 
conditions: x=3000 ft, y=0 ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 44. IBM-PC - Error in torpedo depth during a 10 dg/s 
maneuver in the area of a single array. Initial conditions: 
x=35000 ft, yzO ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 45. IBM "PE - True trajectory of the torpedo during 
a 10 dg/s maneuver through multiple arrays. Initial 
conditions: x=1000 ft, yzO ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 46. IBM-PC —- Estimate of torpedo position during a 
10 dg/s maneuver thruough multiple arrays. Initial 
conditions: x=1000 ft, y=0 ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 47. IBM-PC - Error in torpedo position during a 10 
dg/s maneuver through multiple arrays. Initial conditions: 
x=1000 ft, y=0 ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure ۰ IBM-PC - Error in torpedo position during a 10 
dg/s maneuver through multiple arrays. Initial conditions: 
x=1000 ft, ysO ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 49. IBM-PC — Error in torpedo position during a 10 
dg/s maneuver through multiple arrays. Initial conditions: 
xz1000 ft, yzO ft, Vx=-50 ft/sec, Vy=5 ft/sec. 
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Figure 30. IBM-PC - True trajectory of the torpedo during a 
20 dg/s maneuver in the area of a single array. Initial con- 
01510505: x-9000 ft, y=0 ft, Vx=-92.5 ft/sec, Vy-O ft/sec. 
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Figure 51. IBM-PC - Estimate of torpedo position during a 
20 dg/s maneuver in the area of a single array. Initial 
conditions: x=5000 ft, ysO ft, Vx=-92.5 ft/sec, Vy=0 ft/sec. 
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Figure 53. IBM-PC - Error in torpedo position during a 20 
dg/s maneuver in the area of a single array. Initial 
conditions: x-25000 ft, yzO ft, Vxz-92.5 ft/sec, Vy=0 ft/sec. 
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Figure 54. IBM-PC - Error in torpedo depth during a 20 dg/s 
maneuver in the area of a single array. Initial conditions: 
× 5000ء‎ ft, yzO ft, Vx=-92.5 ft/sec, VyzO ft/sec. 
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Figure 55. IBM-PC — Real time plot of estimated trajectory 
Of the torpedo through multiple arrays. Initial condi- 
tions: x=20000 ft, y=0 ft, Vx=-92.5 ft/sec, Vy=0 ft/sec. 
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Figure 58. IBM-PC - True trajectory of the torpedo in the 
during a 320 dg/s maneuver through multiple arrays. Initial 
conditions: x220000 ft, y=0 ft, Vx=-92.5 ft/s, Vy=0 ft/s. 
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Figure 57. IBM-PC - Estimate of torpedo position during a 
20 dg/s maneuver through multiple arrays. Initial 
conditions: x=20000 ft, y=0 ft, Vx=-92.5 ft/s, Vy=0 ft/s. 
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Figure 58. IBM-PC - Error in torpedo position during a 20 
dg/s maneuver through multiple arrays. Initial conditions: 
x220000 ft, y=O ft, Vx=-92.5 ft/s, Vy=O ft/s. 


B84 


bod 


baa c 

تھے ہے 

FX Lj 

= cs 
oo Ee - 

=] — 7. 

= - 
= >= — 


0.3 


UE 


/ 


= 1 
Cee. = 
A EA = 
متا تا‎ = 
CS ص‎ 
اھ‎ 
cs 
یکت‎ CI ur» Cc Cr“ owl CS 
CJ on co Lu» CJ - - = - c æ 
= = - - - — O3 M = 1 ہج‎ 
Lu» C" C3 = تک‎ 1 i 1 1 i i 


Figure 39. IBM-PC - Error in torpedo position during a 20 
dg/s maneuver through multiple arrays. Initial conditions: 
x=20000 ft, y=0 ft, Vx=-92.5 ft/s, Vy=0 ft/s. 
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Figure 60. IBM-PC - Error in torpedo position during a 20 
dg/s maneuver through multiple arrays. Initial conditions: 
x=20000 ft, yzO ft, Vx=-92.5 ft/s, Vy=0 ft/s. 
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Figure 61. CIE - True trajectory of the torpedo in the 


area of a single array. Initial conditions : x-3000 ft, yzO 
ft, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 62. CIE - Estimate of torpedo position during a 


straight run in the area of a single array. Initial 
conditions: x=3000 ft, yzO ft, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 63. CIE - Error in torpedo position during a 
Straight run in the area of a single array. Initial 
conditions: x=3000 ft, y=0 ft, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 64. CIE - Error in torpedo position during a 
straight run in the area of a single array. Initial 
conditions: x=3000 ft, yzO ft, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 66. CIE - Variance of position error during a 
straight run in the area of a single array. Initial 
conditions: x23000 ft, yzO ft, Vx--50 ft/sec, Vy=0 ft/sec. 
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Figure 67. CIE - Variance of position error during a 
Straight run in the area of a single array. Initial 
conditions: x-z3000 ft, yzO ft, Vx--50 ft/sec, Vy=0 ft/sec. 
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Figure 68. CIE - Variance of depth error during a straight 
run in the area of a single array. Initial conditions: 
x=3000 ft, yzO ft, Vx-2-90 ft/sec, VyzO ft/sec. 
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Figure 69. CIE - Filter estimate of mean-square error 


during a straight run in the area of a single array. Initial 
conditions: x=3000 ft, yzO ft, Vx=-50 ft/sec, Vy=0 ft/sec. 
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Figure 70. CIE - Filter estimate of mean-square error 


during a straight run in the area of a single array. 
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Figure 71. CIE - Filter estimate of mean-square error 
during a straight run in the area of a single array. Initial 
conditions: x=3000 ft, yzO ft, Vx=-50 ft/sec, Vy=0 ft/sec. 
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area of a single array. Initial conditions : x=3000 ft, 
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Figure 73. CIE - Estimate of torpedo position during a 
straight run in the area of single array. Initial conditions 
are: x=3000 ft, yz2121 ft, Vx=-33.8 ft/s, Vy=0 ft/s. 
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Figure 74. CIE - Error in torpedo position during a 
straight run in the area of a single array. Initial 
conditions: x=3000 ft, y=2121 ft, Vx=-33.8 ft/s, Vy=0 ft/s. 
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Figure 78. CIE - Error in torpedo depth during a straight 
run in the area of a single array. Initial conditions: 
x25000 ft, y-2121 ft, Vx-—355.B8 ft/s, VyzO ft/s. 
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of mean-square error 


during a straight run in the area of a single array. Initial 
conditions: x=3000 ft, y=2121 ft, Vx=-33.8 ft/s, 
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Figure 78. CIE - Filter estimate of mean-square error 
during a straight run in the area of a single array. Initial 
conditions: x=3000 ft, y=2121 ft, Vx=-33.8 ft/s, Vy=0 ft/s. 
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Figure 81. CIE - Estimate of torpedo position during a 


straight run in the area of single array. Initial conditions 
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Figure 82. CIE - Error in torpedo position during a 
straight run in the area of a single array. Initial condi- 
tions: x=3000 ft, y=—-3000 ft, Vx=-33.8 ft/s, Vy=33.9 ft/s. 
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Figure 84. CIE - Error in torpedo depth during a straight 
run in the area of single array. Initial conditions: x=3000 
ft, y2-3000 ft, Vx#2-33.8 ft/s, Vy=33.8 ft/s. 
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Figure 86. CIE - Estimate of torpedo position during a 
Straight run through multiple arrays. Initial conditions 
ares x=34000 ft, y=6050 ft, Vx=-84.4 ft/s, Vy=0 ft/s. 
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Figure 87. CIE - Error in torpedo position during a 


straight run through multiple arrays. Initial conditions 
ares x=54000 ft, y=6050 ft, Vx=-84.4 ft/s, Vy-O ft/s. 
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Figure ۰ CIE - Error in torpedo position during a 
Straight run through multiple arrays. Initial conditions 
are: x=34000 ft, y=6050 ft, Vx=-684.4 ft/s, Vy=0 ft/s. 
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Figure ۰. CIE - Error in torpedo depth during a straight 
run through multiple arrays. Initial conditions are: x=34000 
ft, y=6050 ft, Vx=-84.4 ft/s, Vy=0 ft/s. 
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VI. COMMENTS AND CONCLUSIONS 


There are A different versions of the program, 
for the  IBM-PC and for the CIE microcomputer. However, the 
Sequencial Extended Kalman Filter program presented have 
many common characteristics. The execution time per cycle, 
in both machines, is less than the sample interval for the 
data. The program is written in the "C" programming 
language, and it is meant to be reasonably portable. Double 
precision calculations are widely used, and allowed in many 
cases to bound errors without repeating the calculations for 
the same time interval. 

Calculations are performed using an adaptive Q matrix. 
That is, the matrix @ is recalculated each iter st 
instead of using a constant value. The trajectory simulation 
routine was implemented using geometrical approach. The 
tangential speed is — constant during the whole run. 

The Kalman Filter can track through single and multiple 
arrays. The algorithm always uses the coordinates of the 
closest array in the calculations, for the multiple array 
case. There is essentially, in either CIEKR1 and KR, only a 
main program without  subroutine calls. The bidimensional 
matrices were transformed and are used as vectors. Most 
loops were eliminated to keep the execution time within the 


required limits. 


The CIE version can also run in the IBM-PC but the IBM- 
PC version runs only in the IBM since it calls assembly 
language routines to generate graphic output. The IBM 
version is slower than the CIE version and the IBM-PC 
microcomputer must run with its 8087 math coprocessor chip 
to satisfy the speed requirements. Both machines were 
configured with hard disks, and the data files were read 
from and written to the hard disks. Both, the IBM-PC and the 
CIE versions, will run on the IBM-AT. 

There is almost no documentation or software available 
for the CIE microcomputer. The lack of documentation, the 
lack of a good compiler, a full-screen editor, and the lack 
Of graphics capability were major problems in using the CIE. 

The improvement in the execution time was more 
noticeable in the CIE than in the IBM. This is probably due 
to the presence of the math coprocessor chip, 8087, and (or) 
due to a better compiler,in the IBM-PC. The use of the "'-f" 
("fast floating point format") option, when compiling code 
on the CIE, seemed to help both, the speed of execution and 
the precision of the results. 

The programs were tested for torpedo speeds from 20 
knots (33.8 ft/s) to 55 knots (92.8 ft/s) and turn rates of 
up to 20 dg/sec, or maneuvers of up to 1-6. The magnitude of 
the position errors for linear trajectories were maintained 
less than 10 ft in either computer. The CIE could not track 


any of the circular maneuvers (run numbers 6, 7 and 8 in 
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Table 2). In the IBM-PC, keeping the maneuver within the 
tracking radius of a single array (+ 3000 ft around the 
array center), the magnitude of position errors were 
maintained less or equal to 7 feet for an 1-G maneuver and 
less or equal to 22 feet for the multiple array case. 

The high error obtained for the circular maneuvers in 
either the single or multiple array case reflects the 
algorithm inability to follow fast turn circular 
trajectories. In L1J such high errors were reduced to values 
comparable to the errors found in the straight runs, by 
adding the adaptive @ matrix to PKK to produce PKKM1 and 
repeating the calculations for the same time interval. This 
was called "adaptive maneuver". The "adaptive maneuver" as 
implemented (see segment 2, in the algorithm, in Appendix A) 


was found effective for the IBM-PC when the value 10° was 


used for the diagonal elements of the matrix R. However, to 
keep the execution time per cycle within 1.31 sec, the 
number of adaptive maneuvers performed had to be limited. 


Almost no improvement in reducing tracking errors was 


achieved when the value used was 10“. For the later case a 
great deal of degradation in the execution time was observed 
and the time interval was exceeded even when just one more 
iteration (optimum maneuver) was executed. The procedure کت‎ 


not work on the CIE, which could not handle numbers less 


—7 ۱ 
than 10 reliabily. The "adaptive maneuver" feature was 
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utilized only for the runs 7 and 8 (only 4 iterations were 
allowed) and have produced, for the 1-G maneuvers, results 
which are comparable to the results for the 1/4-G maneuvers. 
The single array cases showed smaller errors, meaning 
probably that the technique used to switch arrays can be 
improved. 

An interesting point is the behavior of the curves for 
the diagonal terms of the matrix PKK. The curves PKK_X vs 
TIME SLOT, PKK_Y vs TIME_SLOT and FKK Z vs TIME SLOT, which 
are the mean square errors should grow proportionally to the 
square of the position errors. These are depicted in the 
curves X ERR vs TIME SLOT, Y ERR vs TIME SLOT, and Z ERR vs 
TIME SLOT. When such does not happen it means that the 
filter model used is not a correct description of the 


process. When the value of the diagonal elements of the 


matrix R were 10“, only the qualitative behavior of the 


—10 
nean square errors agreed with the expected. When 10 Was 


used, the quantitative behavior of the mean square errors 
were about the expected. 

Additional tests should be performed using trajectories 
generated from actual torpedo runs on the Dabob test range. 
As a future step, there is the need to adapt the program to 
Eu it with the correct interface formats to access the 
real data, before running it in the real-time conditions on 


the range. 
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APPENDIX A 
PROGRAM DESCRIPTION 
A. OVERVIEW 

There are at this moment, three versions of the Extended 
Kalman Filter program. In either case the simulated 
trajectory must be first computed and the hydrophones array 
times, corresponding to the simulated positions, are stored 
in a file in order to be read in. All three programs write 
the files XKK.DAT, PKK.DAT, and XDIFF.DAT to the disk. 

To avoid delays due to data writing and retrieving from 
disk drives, the data files can be written to and read from 
an electronic drive built in the RAM memory. There is also a 
utility program ہو کو وو کپ‎ > which allows fast graphic 
analysis after the run. 

1. The CIE1.C Program 

This was the first program to run in the CIE 
microcomputer. It reads "clean" (without noise) trajectory 
data from the file TRAJ.DAT and writes 3 columns, 
corresponding to the coordinates x, y, and z, to the disk. 
The code to generate Gausian noise is contained in the main 
program. 

CIE1.C generates 3 data files, XKK.DAT, PKK.DAT, and 
XDIFF.DAT, containing filtered data to be post-processed. 
XKK.DAT, contains 4 columns, corresponding to the time slot 


and the filter estimates, X(k/k), for the coordinates x, y, 
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and z in that order. PKK.DAT contains 4 columns with the 
corresponding values of the time slot and the error 
covariance matrix for the coordinates x, y, and z, in that 
order. XDIFF.DAT contains 4 columns with the corresponding 
values of the time slot and the errors for the coordinates, 
X4, y, and z, in that order. 
Z. The CIEKRi.C Program 

This program runs in the CIE and provides input and 
output similar to the KR.C . It is able to simulate only 
straight line trajectories, due to the lack of an "arc tan" 
function in the CIE s C Compiler, and at present does not 
access the computer internal clock. 

3. The KR.C Program 

This program includes a set of assembly lanquage 
SIE i nas for interfacing with the keyboard, display, and 
output ports of the IBM-PC/AT. It also includes a plotting 
routine, written in C, which calls the assembly routines to 
generate the frame, to print — values, and to draw the 
estimated trajectory. Another assembly language routine was 
added to allow access to the internal clock in the IBM-PC. 
See references [£7] and [81 and Appendix C for details. 

The Kalman Filter section of KR.C needs the files 
TRJ.DAT and ZIC.DAT, generated by. the trajectory simulation 
routine, for inputs. 

The trajectory simulation generates transit times 


data already corrupted with noise. That involves inclusion 
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of the noise generation routine with the "clean" trajectory. 
The clean trajectory can be either a straight line or a 
circular curve. 

The trajectory simulation routine creates the files 
TRJ.DAT, ZIC.DAT, TRJ.INP, and PARAM.DAT. 

The TRJ. DAT contains the data for the clean 
trajectory, corresponding to the time slot, and the x, y and 
z coordinates. ZIC.DAT contains the noise corrupted transit 
times.  TRJ.INP and PARAM.DAT contain information to be used 
by PLOT.C program in order to identify and relate the 
trajectory data and the parameters used in the Kalman Filter 
to the graphic outputs. 

B. DETAILED DISCUSSION 

Since the programs evolved one from the other, only the 
last one, KR.C, will be discussed in more detail. The 
routine that generates the trajectory and the tools 
developed to facilitate the objective analysis of results 
will also be briefly described. 

1. The Kalman Filter Program - KR.C 

The algorithm for the program KR.C is provided as 
Appendix B and the actual program implementation is shown in 
the Appendix E. 

The capability of.tracking trough multiple arrays is 
implemented by simply defining switching points along a 
coordinate axis, and comparing the estimated x-position 


against switching values. The coordinates of the tracking 
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array are then changed in order to always keep the closest 
array tracking the torpedo. Figure 90 shows the coordinate 
system for the multiple array tracking, the location of the 
switching points and the hydrophone location matrix. 

Tests performed using a straight line trajectory and 
keeping the Q matrix constant showed no improvement over the 
case where the adaptive @ is used. 

2. the Trajectory Simulation Routine - TRAJEC 

This routine was developed as an independent 
simulation program, to generate trajectories, where circular 
and linear segments can be combined to produce a desired 
path. 

Given the initial x,y,z, positions and the initial 
speeds and assuming that the vertical speed is zero, this 
routine generates noisy data to represent the "raw" data as 
it would come from the hydrophones. TRAJEC finds the 
position of a point ina circle after a time interval T, 
Given the tangential speed of the point, and وی‎ tha 
desired turn rate or given the centrifugal acceleration. It 
is assumed that the tangential speed is constant during the 
maneuver. Either the turn rate or the centrifugal 
acceleration can be used to determine the radius. 

Figure 91 represents the geometry of the problem. The 
straight line shown is tangent to circle. A point is 


initially moving on the line and at the time t, it is 
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Figure 91 1 The circular trajectory problem. 
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subjected to centrifugal force which makes it turn. The 
point then describes a circular movement generating a circle 


of radius R, and at the time t, +T will be at the position 
X,Y. Assume Y and 7 known speeds, m = a is then the 


slope of the line tangent to the circle that passes at the 


point x sy, and has center at a,b. The angular coefficient 
of the line perpendicular to the circle and passing at the 


point PERA is 


(x, 7a) 7/7 (y, -b)7-—1/7m, so 
«TA mW (y — b) 
0 
and Rê = (x — ay? + (y — b)“ is valid for any point 


on the circle. 


SE poi = 2 m e 
at x, , Y, د‎ Re = EL aj“ + (y, b) 


R° = (m° 2 [0008 


y, - b = * R / sqrt(1 + mé) 
or b = y, t R / sqrt(1 + m°) 
and a = x +m yv a b) 


For any point on the circle 


x — a = R cos ۵ 


y 6ات‎ R sin ۵ 


From the Figure 91 
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= 4. 
ين‎ = tan nm 


8 52 TT. Z AA 


0 = 0 + u A t 


: - 2 2 
Given Vo My 3 V. sqrt (Vg + اس‎ 


Given w 3 R = V. / رن‎ 


Given the central acceleration, g بج‎ wu = g / V 


At the end of the interval, the position will be x, 
y and the speeds will be given by 


A = — ۷۶ sin 9 


۳ = M cos 9 


These equations, when implemented, generate the true 
trajectory. 

The routine also computes the measurement noise 
times. The Gaussian noise is generated by computing the 
linear approximation of the d of the normal curve [9]. 
The input comes from a sequence of uniformly distributed 
numbers generated by the equation below 

r(I + 1) = ri) % 317.mod(1) . 
The resultant sequence is zero mean and is scaled to obtain 
the desired variance. 
3. The Plotting Routine - PLOT 
The plotting routine is contained in the file PLOT.C 


(IBM-PC), and was written as an independent module to be 
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linked with BIOSIO.OBJ, and reads data from the files 
XKK.DAT, PKK.DAT, XDIFF.DAT, TRJ.INP, and TRJ.DAT. 

That is a menu driven program, which provides 
fourteen (14) different options and was designed to simplify 
the analysis of data generated in a torpedo run. The plots 
have their titles already chosen. The scales reflect the 
maximum and minimum values. A reference number and a label 
for SINGLE or MULTIPLE array cases is printed on the plot 
for reference. Linear Ns s sn is used to interconnect 
the sample data points. An optional plot, where the data 
points are simply plotted, is also available. This last 
option would allow the time to be associated to each point 
on the curve. 

In an attempt to provide the CIE machine with 
Similar capabilities, an ANSI driver was also defined and 
tested successfully in the IBM. However, that provides a 
very limited graphics capability, allowing only 80 columns 
by 25 rows of resolution against the 640 by 200 in the IBM, 
in graphics mode. The file KR1.C makes use of the defined 
driver to produce the graphics presentation of the estimated 
trajectory. 

For UNIX systems, like the CIE, one might want to 
consider the "termcap" database of hundreds of encoded 
descriptions for terminal control. However, except in 


unusual circumstances, C's standard-library I/0 functions 


Provide excellent portability and a wide range of display 
features [61] . 
4. BIOSIO.ASM and BIOSIO.OBJ 
These files contain the assembly language code 
(public domain) and the object code (IBM-PC) to interface 
the C programs with the keyboard, display, output ports and 
to access an absolute memory location. 
5. DOSTIME.ASM and DOSTIME.OBJ 
These files contain the assembly language code and 
the object code, respectively, that allow access to the 
internal clock (IBM-PC). 
6. The SIMTERM program 
This is a public domain program written by Jim X 
Eric Holtman that allows one to emulate an HP-like terminal 
to a UNIX system. It is a self contained module that allows 
one to connect the IBM-PC to the CIE microcomputer, directly 
by using the RS-232 output on the IBM and one of: four (4) 
output ports on the CIE. | 
7. The KALMAN.H header file 
It contains the definitions of the program 
constants, initial values of parameters, and the definition 
of the ANSI terminal driver, which can be used in the future 


to generate rough plots on most of the nongraphic terminals. 
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APPENDIX B 


ALGORITHM FOR THE KR.C PROGRAM 


#define CONSTANTS /* used to define constants */ 
#define MACROS /* define macros */ 
#include FILE.H /* include header file */ 
double  xkkL2001; /* declare global variables */ 
main (} /* main prog. declaration */ 

{ /* block/function delimiter */ 


/* DEFINING FILES */ 
FILE *fpxyz, *fopen(); /* declares fpxyz as a */ 


š /* pointer to a file */ 
. /* yet to be open using*/ 
/* fopení() function */ 
/* DECLARATION OF VARIABLES */ 
int ias /* long integers */ 
short 111]]: /* short integers */ 
float x,y; /* single precision */ 
double xx,yy3 /* double prec. reals */ 


/* OPENING FILES */ 
fppzfopen("pkk.dat","w"); /* open file pkk.dat */ 
/* for writing */ 
fpe-fopen("xdiff.dat","w"); /* open xdiff.dat  */ 
fpxyz-fopen("xkk.dat","w")s /* open xkk.dat +/ 
/* INITIALIZATION OF VARIABLES */ 
Initialize HYDROC) 
Initialize gammal] 
Initialize covwl] 
Initialize ۲۳1 ۲ [ 
Initialize 9 1 ل 2و لا ۵ بر‎ 
/* DATA INPUT - INTERACTIVE ۸ 
menu: Put menu 
Get inputs 
Pertorms actions 
Case statement 
1 Run trajectory 
Write run# and xkkmilCl] to trj.inp 
Call trajec to read trj.inp and 
generate noisy trajectory 
Run Kalman filter 
Modify Parameters 
ae Optimization criteria 
b. Single/multiple array 
c. Initial x, Ys © 
d. Initial Vx, Vy 
e. Noise covariance 


CN 
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runs 


f. Error covariance 
g. Redefine maneuver 
- Turn rate based 
- G number 
- Start/end time 
h. Main menu 


/* Run Kalman filter */ 


/* 


Get run # for reference 

Save inputted data into files 

Open files trj.dat and zic.dat for reading 

Initialize QFIND black 

Initialize ٦۳٣٤٢ [ 

Initialize pdumL3 

Initialize خخ‎ 

Prepare screen for real time presentation 
Calculate scaling 
Draw frame 

Get time from internal clock 

START TIME LOOP  */ 

Start time loop : for (kk=Oskk<itime; kk++) 
€ 
Get hydrophone array coordinates 


again: Get time 


If time interval «X 1.31 goto. again 
Update time 
Get true times e true positions (noisy data) 
Read dataL[1 from file trj.dat 
Read 2111 from file zic.dat 
Save true positions in ۳۱۸۵۲ [ 


adapt: /* GET HROW, CALCULATE GAIN, ESTIMATE ۸ 


js: 


/* COVARIANCE OF ERROR SEQUENCIALLY * 
for (i=O;i<4;i++) 
£ | 
Calculate denom using double prec. saqrt 
Calculate hroawLOo1, Hrowl2], ۲۱۳۵۸۲ 4 [ 
Calculate gnumL1, gdenom 
Calculate gill = gain column 
Calculate covariance of error 1م‎ 
for 1 1 205 11 > 255 1 1 رج++‎ 
€ 
calculate pduml] 
9 

calculate pil] 

Calculate measurement prediction 
Calculate zhat=denom/4860. 
calculate zdifflidJd=ziclijJ-zhat 

Compute 3sigma gate 
82۶۸۵۶ 81 ۲01 و 1 12 ۳1۲و‎ 01 2 ( 

/* use single precision sqrt ۷۸ 
gate=3.*(sqrt(p)/4860 + ۳۳۲1 ( 

Edit invalid time measurements 


196: 


Set flag 
Put gain, gilj=0 
Calculate estimate Based on one meas. 
prediction 
for (11=0311<S3iit+) 
xiLiil2xkkmiLiil-*giliil*zdiffLil 
/* Update for next column */ 
for (iizOsii<Bsi1++) 
i 
xkkmitiil-2xiLiiJ 
okkm1Ciij=piliid 
> 
for 411 25 1 1 > 255 11 (جب‎ 
۵ ۲۲/1 1 2 1 <0 1 ]1 1 [ 
if (i==3) goto 156 
+ /% end of js loop ۷۸ 
/% Accept values as updates for time */ 
/* interval ۸ 
Do updates for the time interval 
xkkmiLJ-2xiL]l 
pkkmiL 1zpiL] 
kkL ]zxi[L]J 
مع [ ] عاعا م‎ 1 + 
Recalculate time measurements and form 
absolute value of residuals 
Edit invalid times for adaptive maneuver 
routine 
Compute adaptive Q matrix 
If (icurvezz1) compute adaptive G 
else use constant Q 
Check time 
By pass maneuver if no time left 
Check for valid measurements 
By pass adapt. maneuver if all invalid 
By pass maneuver if not steady state 
Check average zdiffLlil vs. zdifav 
By pass maneuver if avg.<zdifav 
/* Adaptive maneuver */ 
pkkmiľ[ ]JzpkkL 1+1 1] 
/* Reiterate same time slot */ 
goto adapt 
Compute measurement errors 
xdiffLOl2xkkLOJ-truedLro31 
xdiffLi1l12xkkL21-truedL[í]3 
xdiffL2J2xkkL4J)—-truedL43 
Compute switch to change between G 
constant : straight line trajectory 
or Q adaptive : circular trajectory 
Calculate predictions for the next time 
pkkm1L3 and xkkmitl3 


Output results to the screen 
X, y, Z vx vy time values 
plot xkkLOl1, xkkL21 
Qutput results to files 
xkkLJ1,pkkL1,xdiffL3l 
2  /* end time loop */ ` 
Close files 
> /* end main() */ 
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APPENDIX C 
SETTING UP FOR EXECUTION 

A. USER'S NEEDS 

This section describes the actual hardware and software 
utilized to produce the results in Appendix H. 

HARDWARE 

The following describes the hardware utilized and the 
way to interconnect the IBM-PC and the CIE microcomputer . 

IBM-Pc : 2 floppy disk drives, 10 megabytes hard disk, 
graphic board, graphic color display, serial port (RS-232), 
8087 math coprocessor chip and Epson parallel printer: 

CIE 680/30 : 20 megabytes hard disk, 4 serial output 
ports, Amdek monitor and keyboard. 

The computers were interconnected by cable, from the IBM 
RS-232 output port to one of the 4 ports on the CIE. 

SOF TWARE 

The files and/or programs utilized are: 

1. SIMTERM - communications program (in the IBM). Allows 
transferring files between the IBM and the CIE. 

e CIE.BAT = batch file (IBM). Executes the necessary 
sequence to interconnect the IBM and the CIE. Needs SIMTERM. 

5. KR.EXE - executable file that contains the program to 
simulate the torpedo trajectory and the Kalman filter 
program (IBM). The trajectory routine is executed directly 


from the KR.C program as a menu option and it generates the 


data files "trj.dat", and "zic.dat". Then the trajectory and 
time information from the files, "trj.dat" and "zic.dat", is 
used by the Kalman filter algorithm to produce estimates. It 
generates the files  "xkk.dat",  "pkk.dat",  '"xdiff.dat", 
"trj.inp" and "param.dat". At the end of the run, a total of 
7 files would be generated. 

4. PLOT.EXE - executable file that contains the plotting 
routine (IBM). Allows posterior graphic analysis of the 
results. Uses the information contained in the files 
"Xkk.dat", "pkk.dat","xdiff.dat","trj.dat" and "trj.inp". 

S. | KAL. BAT = batch file (IBM). Executes the necessary 
sequence to run KR.EXE and PLCT.EXE. | 

ó. KALMAN. H - header fils (IBM and CIE). Contains 
definitions used by CIEKR1.C , KR.EXE, PLOT.EXE, TR.EXE . 

7. a.out - executable file generated from compilation of 


CIEKR1.C on the CIE machine. 


B.  PROGRAMMER'S NEEDS 

It is highly recommended to have a good full-screen text 
editor, and the source code files included in the directory 
of the IBM-PC. Source code files have extension ".C". A good 
C compiler (IBM) is also recommended. 

The software required is: 

1. LATTICE C or MICROSOFT C compiler ; 

2. FULL SCREEN EDITOR * 


لیے 


3. SIMTERM - terminal emulation / communication program; 


4. SOURCE files : TR.C, KR.C, PLOT.C ; 
5. HEADER files : KALMAN.H, MATH.H, STDIO.H ; 
6. ASSEMBLY LANGUAGE ROUTINES: BIOSIO.ASM, DOSTIME.ASM ; 

7. IBM MACRO ASSEMBLER ; 

8. MICROSOFT LINKER . 

Using the Lattice C compiler requires the files LC1.EXE, 
LC2.EXE,  LCS.LIB,  LCMS.LIB, CS.UBJ. 613:68: 51055 250 
present. The assembly language routines must first be 
transformed into ےٹک‎ files by using the Macro Assembler. 
The file TR.C must also be compiled using LCi and then LC2 


to generate the TR.QBJ file. These are all linked to the 


file KR.OBJ to produc the executable file KR.EXE . 


APPENDIX D 
COMPILING, EXECUTING, TRANSFERRING FILES, AND PLOTTING 
A. RUNNING THE PROGRAM ON THE IBM-PC 

This will assume one is running the Kalman Filter 
program on the IBM PC/AT with a hard disk drive defined as 
drive "c".. The system prompt is "c>", and the required 
files are in the subdirectory named "KALMAN". 

If you are not in drive "c", change to the correct drive 
typing "cs" and hit "RETURN". The system will then answer 
with the prompt "c>", Type "cd kalman" to access the 
directory KALMAN . 

1. Compiling KR.C 

This will assume you are running the Lattice C 
compiler. Type "ce kr" to compile the file KR.C, using the 
batch file "CE.BAT". The files KALMAN.H, DOSTIME.OBJ, 
TR.OBJ, BIOSSIO.OBJ, LINK.EXE, CE.BAT, and KR.C need to be 
in the same directory. 

The Appendix E contains a listing of the file 
CE.BAT. 

2.  Executing KR.EXE and PLOT.EXE 

Type "KAL",in drive "c", to execute the batch file 
KAL.BAT. The required sequence of commands is initiated and 
can be interrupted by pressing "CTRL-BREAK" ("CTRL" and 
"BREAK" keys at the same time). The Appendix E contains a 


listing of the file KAL.BAT . 
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The first file to be executed is GRAPHICS. It allows 
dumping IBM-PC graphic screen to the printer. 

Next the file KR.EXE, containing the trajectory 
routine and the Kalman Filter program, is executed. Menus 
Give the indication for data input, to run the trajectory 
routine, and to run the Kalman Filter. One normally defines 
the parameters first (option # 3), then run the trajectory 
routine (option # 1), and then run the Kalman Filter (option 
# 2). | 

After the run, the plotting routine ("Plot") is 
called to produce the graphic output. Up to fourteen 
different plots can be generated by making the desired 
selection from the menu. The. output to the printer is 
obtained by pressing "SHIFT-PRTSC" ("Shift" and "PrtSc" at 
the same time). 

If a plot ends abnormally, for instance by the use 
of CTRL-BREAK, the screen is left in the graphics 640 by 
200 mode, then the command "mode bw80" will set the display 


mode to Black and White with 80 characters per line. 


B. RUNNING THE PROGRAM ON THE CIE 

Turn on the system with the floppy disk drive open. This 
will force the system to boot and accept the internal hard 
disk drive as the current drive. Turn on the terminal 
display and when the cursor appears, press "ENTER". Follow 


the prompts for date and time input. 
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After booting, the screen will show the message "CIE 
GRAPHICS SYSTEM # 7" at the top, and "login : " at the 
bottom. At this moment one should have the SIMTERM diskette 
in drive A (the one at the left) on the IBM-PC. If the 
logged i is not A, one should type "a:" and press 
"ENTER" to change to drive A. In drive A, one should type 
"cie" and "RETURN" (ENTER). The batch file CIE.BAT will send 
the correct command line to connect the IBM and the CIE. 
Make sure the cable connection is already done. 

The message "connection established" will then appear on 
the screen. Press "ENTER" to get "login:". Log in using the 
account "jose", and press "ENTER" for the password. The 
system prompt is "%" . Change directory using "cd work" . 
One is then ready to transfer files or to execute a program 
in the CIE machine. 

1. Transferring Files from the IBM to the CIE 

To transfer a file use the redirection capability of 
the Unix system and the command "cat". 

Type "cat > filename.ext" and press "ENTER", at the 
system prompt ("X"). The cursor will stop at the begining 
of the next line. "ALT-D" will dump a file to the 
communication line. One will be prompted for the filename. 

If, for instance one wishes to transfer the file 
" ۱51 ۵۳۰۲۲ و‎ which is in drive cs (IBM), to the CIE, given it 
the name “kalman.h", the following sequence should be 


performed, starting at the prompt "A" : Type "cat > 
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the name "kalman.h", the following sequence should be 
performed, starting at the prompt "Z" : Type "cat > 
kalman.h"; press "ALT-D"; type "c:kalman.h" , when asked for 
the IBM filename, and press "ENTER" to start the 
transmission. 

At the end of the transmission the computer bell 
will ring, signaling buffer overflow. One should wait until 
no more new lines are being written. Press "CTRL-D" to close 
the file in the CIE. The prompt "X" will reappear on the 
screen. 

The new file (kalman.h) will have to be edited on 
the CIE to remove the inserted empty lines: 

Type "ed kalman.h" and the editor will show the 
number of lines in the file. Then, type "g/^$/d" and press 
"ENTER", The number of lines in the file will be reduced to 
the half. This will not affect the blank lines — as 
in the program, just the ones inserted by the process of 
transmission. 

Type "w" to write the modified program version to the 
disk, and "q" (quit) to leave the editor. 
Z. Compiling on the CIE 

Type the command line "cc -f -v -L -7 ciekri.c" to 
generate the executable file "a.out". 
where : "cc" invokes the C compiler; 

t option , informs the compiler to use "fast 


floating point format": 


HSV AMOS CI or m 
سنا‎ option instructs the compiler to generate 
pointers to allow long jumps, for a codetdata greater 
than 32 kbytes; 
۰ - option instructs the compiler to use the Unix 
version 7 standard library. 
5. Executing )1 1 
Type "a.out" to start execution and then follow the 
menus for inputs. Since the  CIEKR1.C program does not 
generate circular trajectories, the files containing the 
trajectory data  (TRJ.DAT) and transit time data (ZIC.DAT) 
will have to be transferred from ns IBM to the CIE, in 
order to run the Kalman Filter for the circular trajectory. 
Be sure to input the same data that were used to generate 
the circular trajectory on the IBM, and then select option 
#1 (Run Kalman). For straight line trajectories, just follow 
the menus. 
4. Plotting the Results 
Since there 1s no graphics capability built in the 
CIE, the only way to obtain plots is to transfer the files 
XKK.DAT, PKK.DAT and XDIFF.DAT, obtained on the CIE, back to 
the IBM and then use the routine PLOT to generate the 
desired graphic output. The procedure is described below: 
"ALT-C", during the SIMTERM session, will open a 
capture file. One will be asked the name of the capture, or 


log file. The drive name should be specified. 
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For instance, assume one wants to transfer the file 
XKK.DAT from the CIE to the IBM in order to use it later to 
generate plots. Since, the routine PLOT reads specific 
files, be sure to erase the old file XKK.DAT, in the IBM, 
before copying the file XKK.DAT from the CIE, because the 
same name will be used. 

Type "list -n xkk.dat", at the prompt "X", without 
pressing "ENTER". Pressing "ALT-C", will start the capture 
mode. Then one will be asked the name of the log file. Type 
"csxkk.dat", and press "ENTER", to open the file on drive C. 
The command line "list -N ... " will reappear. Press 
"ENTER" to start the "02 2 and the capture. Press "ALT- 
C"at the end of the file listing, to close the log file. 

After doing similar procedure for the files 
XDIFF.DAT and PKK.DAT, leave the account on the CIE by 
typing "logout". 

Leave the SIMTERM program and drop the line, 
pressing "ALT-F2". One will be back to drive A on the IBM. 

Type "cs", to change logged drive to GC a` 

One may use his preferred text editor to eliminate 
the undesired lines in the files XKK.DAT, XDIFF.DAT, and 
PKK.DAT. The files should contain only four columns of 
numbers, except for the first row, which is blank. Then one 
can use the routine PLOT to obtain the desired plots, by 


typing "PLOT =8000", and following the menu options. 
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APPENDIX E 
KR.C PROGRAM LISTING - IBM-PC VERSION 
This Appendix contains a listing of the Extended Kalman 
Filter program implemented in the IBM-PC microcomputer, as 
well as the listings of the Trajectory Simulation routine 
and the header file "kalman.h", which are used by the main 
program KR.C . The other header files declared at the 
begining of the main program are standard files contained in 


the C compilers. 
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/ 3 Je 3€ 3€ 3e 3€ 3€ 3€ Je 3€ Je 3€ 3€ 3€ Je 3 3€ Je 3€ 3€ JE 3€ 3E 3€ 3E 3€ 9€ 9€ 9€ 9€ 9€ 3€ 3€ 9€ 3€ 3€ 3C 9€ 3€ 3€ 9€ 3€ 9€ 3€ 9€ 9€ 3€ 9€ 90 9€ 9€ 9€ 3€ 9€ 9€ 3€ 9€ 3€ / 


/* */ 
/* HEADER FILE - KALMAN.H */ 
/* */ 
/ 3C3C 3C 3C 3C E 3e 3e 3e 3C 3€ 3€ 9e 9e 36 3€ 3€ 9€ 9e 96 9€ 9€ 9€ 3€ 36 3€ 3E Je 3e 3e 3€ 3E JE 3C 3C 3E 3E 3E 3e 36 3C E 3e 3C 3C 3E 3E 3E 3C 3C E 3E 3€ 3€ 3E 9€ کت جل جل‎ 
#define nmax 5 /* * of col. in pkk, q ... %/ 
#define maxsq 25 /%# size of equiv. vector PKK,Q...*/ 
#define js 4 /* # hydroph. in any hydrophone ۷ 
/* array */ 
#define hcmax 12 /* # of col. in matrix HYDRO of */ 
/* hydroph. coord. */ 
#define hmax 72 /* equiv. vector size of HYDRO */ 
#define dmax 3 
#define itime 150 /* max.# of time slots */ 
#define npt 130 
#define icmax 5 /* max.# of optimal iterations */ 
/* allowed */ 
#define criter 0.1 /* criteria to compare ZDIFAV ۷۸ 
#define na O /* may assume ONLY values O or á ۷۸ 
f /* Q =>single;ó=> multiple arrays */ 
#define xpos 10000.0 /* initial torpedo x-coordinate */ 
define xvel - 50.0 /* initial torpedo x-velocity */ 
#0 6۴ 1 ۱۶ ۷۳5 ۰۵ 0 /* initial torpedo y-position ۷۸ 
#define yvel - /* initial torpedo y-velocity */ 
#define zpos ٥ /* initial torpedo z-position */ 
define swO 33000.0 /* swi define points where */ 
#define sw1  27000.0 /* switching from one to */ 
define sw2 21000.0 /* another */ 
define swž 15000. 0 /% array */ 
#define sw4 9000.0 /% is done. */ 
#0 6+ 1 ۲2۶ 51 001۷ 1.0 
#define scc 0.387443094 /* - in radians... */ 
#define sacc 36.2 
#define vel 4860.0 /* speed of sound */ 
#define twopi 6.28315308 
#define rrvar 1.e-5 
#define rvar 1.e-10 
#define pkkdiag 1000.0 
#define limvar 1.5 /* defines threshold to use ۶7 
/* constant  */ 
#define gmO 0,86 
#define gm1 1.31 
#define cwO 1.0 /* initial was 1036.84 ۷۸ 
define cwl 1.0 /* initial was 1036.84 */ 
#define cw2 1.0 
#define xminl -5000.0 
#define xminh 0.0 
#define xmaxl 5000.0 
#define xmaxh 40000.0 
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Hdefine yminl -5000.0 
#define yminh -5000.0 
#define ymaxl 5000.0 
#define ymaxh 5000.0 


/ 3t 9&9 9€ 96 396 3€ 96 9 9 36 2 9€ 3€ 96 3€ 9 29€ 9 3€ 29€ 9€ 9t 9 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 39€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9E 9€ 9€ 39€ 9€ 9€ / 


/* */ 
/* i DEFINE ANSI DRIVER * / 
/* */ 


9€ 9€ جد 9€ 9€ 90 39€ 9€ 9€ 9€ 9€ 9€ 96 9€ 39€ 9€ 9€ 96 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 9€ 9€ 2€ 9€ 9€ 9€ 9€ 9€ ہد 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 9€ 3€ 9€ 9€ 3€ / 


/* #define STDIN 0 */ 

/* #define STDOUT 1 */ 

/* #define STDERR 2 ۷۸ 

#define CLR_SCRN fputs("\33C2J", stderr)? 

#define CUR_MV(row,col) fprintf(stderr, "\3304%d3%dH", row, 

col) 

#define CUR_UP (num) fprintf(stderr ,"\330%dA", num) 

#define CUR_DN (num) fprintf(stderr, "\3304dB", num) 

#define CUR_RT (num) fprintf(stderr, "\S33CZ%ZdC", num) 

#define CUR_LT(num) fprintf(stderr, "\330%dD", num) 

#define CUR_SKIP fputs("\n", stderr) 

#define CUR_SAVE fputs("\330s", stderr) 

#define CUR_REST fputs("\33Cu", stderr) 

#define CLR_LINE fputs("\33EK", stderr) 

#define BELL fputc(’\7’, stderr) 

Hdefine CLR EOS(r, c).íbyte i ; CUR MV (rc); A 
for (i_=r; i <=253++i_) CLR_LINE, CUR_DN (1); 

CUR_MV(r,G)3 + 
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/ 3€ 3€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9 9€ 9€ 9€ 9€ 9€ 9 9€ 9€ 9€ 9€ 9€ 3€ 9€ 9€ EEE HEHEHE / 


/% */ 
/* REAL TIME KALMAN FILTER PROGRAM - KR.C عد‎ / 
/* */ 


3C 9€ 3€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ €‏ 3€ 3€ 3€ 3€ 3€ 3€ 3€ 96 3€ د 3E 3€ 9€ 9€ 3€ 9€ 9€ 9€ 9€ 30 3€ 3e‏ 96€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ ېډ ېډ ېډ ېډ د ار 


Hinclude "stdio.h" 

Hinclude "math.h" 

#include “kalman.h" 

double 

و 1 0034 ]21 031 و 1 ۲۲۲۲۱۸۵۸ و ل 56 ۲۵ ]۵۲۲ و 1 56 63۵ ] ۵۲۲01 و 1 ۲۳۱۵۲ ۲۱61 » 


/* STARTING THE MAIN PROGRAM... */ 


main () 

< 
/ چد چد چد چد‎ HHH HHH HH HH 9€ 9€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9 9€ 9€ 9€ 3€ 3 9e 9€ 9€ 9€ 3€ 9 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9 9€ 9€ 3€ 9€ 9€ 9€ / 
/* */ 
/* VARIABLE DECLARATIONS */ 
/* عد‎ / 


/ 3€ 3€ 3€ 3€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 


FILE *fpxyz, *fopen(); 
FILE *fpe, *fopen(); 
FILE *fpp, *fopen(); 
FILE *fptrj, *fopen(); 
r FILE *fpzic, #fopen(): 
FILE *fparam, *fopen(); 
FILE *fpin, *fopen(); 


int ihours,imins,isecs,iths,col,row,maxopt; 
int i8,i,Jj,kk,iin,icont,icurve,option; 

int param,tr jopt,nrange; 

int ii,jj,narray,nzdiff,in,pli,runn,c; 

int jstart,jend,worg; 

float rabs,zgate; 

double height,width,ymax,ymin,xmax,xmin; 
double xscale,yscale,bx,by; 

double hours,mins,secs,ths,time,oldtime,deltat; 
double sqrt(),oldx,value,xO,y0,zO0,vxO,vyO:; 
double denom,ai4,gdenom,gate,zdifav; 

double den,velm,temp; 

double a2,91,92,93,p1,xdiff[321; 

double zhat,zcs,dk,sigcc,sigacc; 

double v,truedti3J,wd,g; 

double x,y,z,auxh,xd,yd,zd; 

double al,e1,e612,e2,bb,dd,bd; 

double pkl,pk3,pkS,pgate,rgate,p,p11; 

double phipkkfmaxsq2J,pktemptmaxsq1,qtmaxsq1; 
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double 
double 
double 
double 
double 


piLmaxsql,pdum[maxsqJ,qtempLmaxsq1; 

gnumLnmaxJ,gilnmaxJ,xiLnmaxl,hrowLnmax 1j 
zicLjsJ,ziLjsJ,zdiffLjsJ,rLjsJ,phisLjijs1,ztL js1; 
xbCisl,ybCjis],zbCjsi,rrfCisi; 
hydroLhmax J,covwL31,gammaL21,o0ptt 31; 


/ 3€ 3€ 4€ 3€ ہر‎ Je Je Je 36 36 Je Je ہو ہو‎ 36 3€ 3€ 3E 3E Je Je 36 Je 36 36 3€ JE JE 36 JC JE Je JC JC 36 JE JE JE Je JC JC JC JC JC JC JC JC JC 3C JC JC JC JC 36 36 3€ 36 3€ ر‎ 


/% 
/% 
/* 


INITIALIZATIONS 


*/ 
*/ 
*/ 


e 3e 3C 3C 3e 3C 3C 3C 3C JC JC 3e YE JC 3C 3e 3C JC JC Je Je Je Je Je Je X6 Je e e Je e Je Je e Je Je Je Je Je Je Je Je Je Je Je Je JC JC Je Je Je Je Je Je Je Je 36 36 /‏ از 


*/ 


fpxyz-fopen("xkk.dat","w"); 
fpe=fopen("xdiff.dat","w"); 

fpp=fopen("pkk.dat","w")3 

/* INITIALIZE CONSTANTS, HYDROPHONE MATRIX, PHI, R, 
DATA FOR TRAJECTORY, AND LOAD xX(O/-1)}, P(O0/-1) 
nzdiff-4; icont=0; icurve=13; value=criter; wd=5.730; 
runn=15 narrayzna: paramzOs worg=13; g=0.1563 nrange=0; 
jstart=403; jendz80; i820; maxopt=icmax ؤ۶‎ 
hydroLOoJ236000.0; ۳۷۵۳۵] 1 [2 6000.03 hydrof2]= O. O; 
hydroL3123605350.0; ۲۷/۵/۳۵۲4 152 6000.03 hydro[51= 0.0; 
hydroLó61256000.05 hydrol7]J= 6030.03 ۳۱۷/۵۳۵] 8 1< O. O; 
hydroL91236000.0;  hydroLi1O0126000.0; hydro[11l= 30.0: 
hydro[12l1=šOO000. O; hydroL[L1l13l=6OOO0.O; hydroli4]= 0.090; 
hydroL13512350050.0; hydroLi1ó6126000.0; hydroLi1715 0.0; 
hydroL181230000. 0: hydroL191726030.0;5 ۲۱۷۵۳۵۲2012 0 5 
hydrol213=30000.03 hydrol221=6000.03 hydrol23]= 30.0; 
۳۱۷/۵۳۵۲24 1224000 ۰ 45 hydro[ľ 25126000, 05 hydroľ26]2 0.0; 
۳۱۱/۵۳۵۲27 1224030۰ 4۵5 ۳۱۷۵۳۵] 28 126000۰ 05 ۳۱۷۵۳۵۲29 12 0.0; 
hydroL3501224000.05;5 hydroL3513226030.0; hydrolfS2]= 0.0; 
hydroL331224000.0; hydroL34126000.0; hydrol3SJ= 30.0; 
۳۱۷۵۳۵۲56 121 8000۰ ۵۶ ۳۷۵۳۵۲57 126000۰ 4۵5 ۰ ۳۷/۵۳۵۲5812 0.0; 
hydrols91=18030.03 hydrol40]=6000.03; ۳۷۵۳۵۲41 [2 0.0; 
hydroL 421218000. 05 hydroľ 43126030. 05 hydrol44]= 0.0; 
hydroL452218000.0; hydroL46126000.0; hydro[47]- 30.0s 
hydroľĽ 481212000. 0: hydro[ 4926000, 0: hydror501s 0.0: 
۳۱۷/۵۳۵۲51 121 2030۰ 4۵ hydroL 52126000. 0s hydroC53]7- O. 0s 
hydroL543212000.0; hydroL552126030.0; hydroL[Sóol1-2 O. O; 
hydro[571=12000.O; hydroL[581l=6&6O000.0O; hydrolS59]= 30.0; 
hydroL60 ]s6 000. 0:3 hydrolé11=6000.03 hydrolé2]= وت‎ 
hydroLé35126030.0; ۰ ۳۷/۵۳۵] 64126000۰ 0: ۳۷/۵۳۵] 65 < 0.0: 
hydroLó66126000.0; hydroaľ67]26030. 0s: ٣٦٦۷۹٦1۴6 007 0۰ 0 
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hydroLó69226000.0; hydrol703=6000.03 hydrot7112» 30.0; 
rtOolJ2rvar; rLií112rvars; ۳۲۸۵152۳۷۵۳ ۳5 [2۶۳۱۷۵۳ 


98 500066] 01529060: 951000] 1 1291 : 
covwLOJ2cwO; 20۷۷] 1 122۷۷1 20۱۷۷۷] 2 12۷2 


xO=xpos3; vxO-2xvel; yO=ypos; vyO=yvel; 
2 22 95 و‎ 


for (i=O;i<nmaxi;i++) 
for (7203 j<ánmax 3 j++) pkkmiLi*nmax-*j120.0; 
for (1=O03;i<nmax3itt+) pkkmiĽľ1+#nmax +1 Jzpkkdi ag: 


phisfCOJ=1.313; دام‎ 51 130.05 phist2121.31; 6۵۲۱1 51] 3 120۰ 


xbCOJ=(-15.0); yblOIJ=(-15.0); zbLOI=(-15.0); 
5 ] 1 12 15. 05 و (0 .15-) 2 25]1 1 (15,0-) 212 1] کب‎ 
۷۲ ]21<2 )-15, 0( :  ybt212 15.0; 2 ] 23 )-15. 0( و‎ 
۷ ] 3532 )-15,0( :  ybEt315(-15.0); zbt315 15.0; 


/* INITIALIZATION FOR QFIND 7۸ 
velm=1.0/vel; 
al4=phisf[0]; 
az=a1l4*al4; 
gi-a2*0.9; 
g2-2gl*gl; 
g32al4*gl; 
Sigacc=sacc*sacc;3 
S1gCC—SCC#SCC: 
for (12031<(nmax31++) 
for (j=03 j<nmax3 j++) qLi*nmax-*tj120.0; 
qLmaxsq-112sigdiv*aZ*sigdiv; 


/* INITIALIZATION FOR HROW */ 
۱۱۳ ۵۷۷] 1 1=0.0; ۲۱۳ ۵۷] 5 [ 20 . 


/ #* INITIALIZATION FOR PDUM */ 
for (11=03;11<nmax3iitt) 


t 


[ ز‎ 21 1 #۳۱۵۷ 3 
pdumt [+1 0.0: 
pdumt j j*3220.0; 
+ 
pdumLó5121.0; pdumEL1832-721.0; 


/* INITIALIZING RR */ 
for (1120311<(4311++) rrtiil-sqrt(rLtiil); 
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3F 3 3F 3 9€ 9€ 9€ /‏ د ڄڊ طز طز 9E 9E E EE EE‏ 3 3 د 3 چڊ د چڊ جد چڊ چڊ چڊ چڊ 9E‏ جد جد جد د چڊ د جد د مډ جد چڊ چڊ چڊ چڊ چڊ ہد ود جج 33630909 / 


/* */ 
/% DATA INPUT * / 
/* */ 


E ARA RC‏ جر چا و و ور وی جر جر وچ ج چ چ ج ج چ چ چ چ چ چچ چ چچچ ب چ چ چ چ چ چ چ کد چ حأ فد عد فد أ جد چ چو چ چ چچ چ د چ چچ ر 
restart:‏ 
و ۷ < [ ق ] ۱۱۰۸1 2۷۵ [ 2 ] ۱۱۰6۸1 ون 12:۷ 1 ] ۱۱001 xkkmiLOJ2x0:;‏ 
:20 [ 4 ] ۲:۲1 7 


menu: /* OPTIONS MENU ۸ 

printf ("hn s 
printf ("An KALMAN FILTER "): 
printf ("\n"); 


printf ("An OPTIONS :")3 

printf (Amins 

printf "An 1) RUN TRAJECTORY"); 
printf ("\n") 3 

printf ("An 2) RUN KALMAN"); 

printf ("Mn"); 

printf ("in 3) MODIFY PARAMETERS"); 
printf("Xn"); 

printf ("An 4) QUIT"); 


printf ("yn"): 

if (param!=0) 
{ 
printf ("yn"); 
printf ("\nA parameter was changed ... You may "); 
printf ("have to run TRAJECTORY before continue. "); 
>+ 

printf("NnEnter value (1-4) : "); 

scanf ("Zd",%option) ; 

if (option==1) 
t 
fpin-fopen("trj.inp","w"); 
fprintf(fpin,"Xd XAd'",narray,runn); 
fprintf (fpin,"\nZe Ae Ze", 

xkkmiLOJ,xkkmiLilJ,xkkmi1L232); 

fprintf(fpin,"NnAe Ze" ,xkkmi[ 3], xkkm1[4]): 
fclose(fpin); 


trajec(narray,hydro,xb,yb,zb,xkkml,jstart,jend,wd,g,worg?); 

par am-O; 
goto menus 
> 

else if (option==2) goto run; 

else if (option==3) goto modify; 

else if (option==4) goto quit; 

else goto menu; 
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modi fy: 


0 ٤ MODIFY PARAMETERS OPTIONS و( ''ع‎ 
printf ("\n") 5 i 

printf ("\n 1) ZDIFAV stop optimization"); 
printf(" criteria"); 

printf ("\n 2) SINGLE / MULTIPLE ARRAY"); 

printf ("\n 3) INITIAL X, Y, and Z positions"); 
printf ("\n 4) INITIAL Vx and Vy velocities"); 
printf ("\n 9) NOISE COVARIANCE — diag elem of"): 
printfi mi 

printf ("\n 6) MAX. # ITERATIONS OF OPTIMAL); 
printf (" MANEUVER"); 

printf (“"\n 7) REDEFINE MANEUVER - w / g / time); 
printf(" interval"); 

printf ("\n ©) USE LOW / HIGH RANGE "); 

Drimet ("Nits 

printf ("\n 9) MAIN MENU — OPTIONS"); 


printf ("\n")s 
printf ("\nEnter value (1-9) : "); 
scanf ("“Zd" ,&param) و‎ 


switch (param) 


€ 
case 1: 
printf ("\n ^S 4e", "Current CRITERIA iss", 
value); 
printf("\nchange ? LY/NJ s"); 
scanfi("Z4s",&c); 
if (c2-'n' ti c=='N’) break; 
printf ("\nEnter new value (real) s ")5 
scanf ("Z2lf" ,&valued و‎ 
break; 
case Z2: 


printf("NnSINGLE array is the default"); 
if (narray==0) ` 
printf ("\n“~s", "Current case is : SINGLE"); 
else printf("knZs", "Current case is: MULTIPLE"); 
printf ("4nchange ? LY/NI :"'"); 
scanf("4s".%C)5 
if (cz2“n“ it Cz“ N“) break: 
printf ("knEnter S for SINGLE or“): 
printf(" M for MULTIPLE array و‎ 
5 5۳04+ ۳۸۵۵5 56۱ $3 
if (cB='’s’ tt C=='S') narray=0; 
else if (c=='’m’ ti c=='M’) narray=1;5 
else narray=0; 
break; 


# 
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case 3: 

printf ("\nCurrent values are : "); 

printf ("\n%4s Ze Xs “Ze As “Ze","xpas = ",xkkm1COl, 
" ypos = ",xkkm1L21," zpas = ",xkkm104]); 

printf ("\nchange ? [Y/N] :"); 

scanf ("4s",&c) و‎ 

if (c=='n’ ۲۱ ۲ c=='N’) break; 

printf ("\nEnter new values : ");; 

printf("WMnZAs","xpos < و("‎ 

scanf ("Z21F",&xO) $ 


printf ("\nds","ypos = '); 
scanf("Z1f",£yO); 


printf ("\nd4s","zpas = و("‎ 
scanf ("A1LF" ,&zO) § 


xkkmiLOl12x0; xkkm1L21-2y0; xkkmi[41220; 
break; 


case 4: | 
orintf ("\nCurrent values are 
printf ("lnZs Ze As Ze", "xvel 

"  yvel = ",xkkmi1L32);j 

printf("Nnchange ? [Y/N] :"); 
scanf("Zs",&c);5 - 
if (c=='n’ ii c=='N’) breaks 
printf ("\nEnter new values : "); 
printf ("\nds","xvel = و("‎ 
scanf ("A2lLF" ,&VxKO) ¢ - 


is 


" xkkmitil, 


printf("NnAs","yvel = "); 
scanf("Al1f'",£&vyO); 


xkkmiLil12vxOs:; xkkmliL[3l12vyO; 


break; 
case Ss 
orintf ("\nCurrent values are : "); 
printf("NnAs Ze 4s Ze ~s Ae As ۵۷ و" = 4۵1 ]۲۳ و‎ 
Ero ne سے‎ ۱۳ = ried. 
"oan [p «m ir hsv 


printf ("\nchange ? [Y/N] :"); 
scanf ("%s",&c); 

if (c2-2'n' ii czs2'N') break; 
printf("NnEnter new values : "); 
print fin s "br 0] = "335 
scanf ۳۸۵ ۶ ۲ و‎ 5۳] ۵1۱ ۶ 


1 


printf("NnZs","rf121 = "); 
scanf ("%14",&r£1I)3 


printf ("\n/4e","rl2d = “de 
scanf ("A1f",£&rL22); 


printf("NnAs","r L.S qs 
scanf ("A1f",&rC32)5 


for (11=0311<4311¢+) rriíiil-sqrt(rtiil1); 


break; 
case 6: 
printf ("\nCurrent value is : و("‎ 
printf("NnAs >0” ٭'',‎ of iterations = ",maxopt); 


printf ("\nchange ? LY/N3 :"); 
scanf ("A4s",&c); 


if (c2-2'n' {tt c=B="N’) break: 
printf ("\nEnter new value : و("‎ 
printf ("\n“zs","maxopt # = و("‎ 
scanf ("A4d",&maxopt) ; 
break; 
case 7: 
trmenus printf ("\n DEFINE MANEUVER n"); 
printf ("\n 1) Define maneuver based"); 
printf ("on turn rate"); 
printf c¢"\n 2) Define maneuver based"); 
printf(" on g-number"); 
printf ("\n J) Redefine start and end"); 
printf" time slots"); 
printf ("Nn 4) Modify menu"); 


printf ("\n\nOption +: "); 
scanf ("Z%d" ,&trjopt)s; 

if (trjopt==4) break; 

if (tr jopt==1) 


1 
printf("NnCurrent value is : و("‎ 
printf("A4s Ze","turn rate : w= ",wd); 
worg=1; 
printf ("\n Enter new turn rate (dg/s)1'); 
scanf("Zlf",£&wd); 
> 

else if (trjopt==2) 
1 
printf("NnCurrent value is : و("‎ 
printf("Zs Ze","g-number : g > ",g)5 
worg=0; 
printf ("An Enter new g-number : '"); 
scanf("Z4lf",£&g); 
y 
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else if (tr jopt==3) 
c 
printfí("NnCurrent values are s: و("‎ 
Printf("\n%s Zd 4s XAd","tstart = و"‎ 


printf ("jstart,"tend = ",jend); 

printf ("\n Enter start time slot : "); 
scanf ('"Zd",£&jstart); 

printf ("\n Enter end time slot : "); 
scanf ("Zd" ,xjend)? 3 

+ 


goto trmenu; 


case Bt: 
printfí("NnLOW range is the default'); 
printf(" (use single array)'); 
if (nrange==0) 
printf ("AnZs", "Current case is : LOW"): 
else printf("NnZs", "Current case is : HIGH"): 
printf ("Jn nchange ? LY/NJ :"); 
scanf("4s",%C)5 
if (c2-'n' ii| cz22'N') break; 
printf ("\nEnter L for LOW or"); 
printf(" H for HIGH range : "); 
scanf("Zs",&c) 3 
if (0221 it cez CL“) nrange=0; 
else if (c22“h ii c=='H') nrangezi: 
else nrange=9; 
breaks 


case 9: 
goto menu; 
break; 


default : 
break; 

+ 

goto modify; 


run:  /* START EXECUTION 7۸ 
printfí"Nn"); 
printfí("NnEnter run d : و("‎ 
scanf ("Ad" ,&runn) ; 


fparam=fopen("param.dat","w'"'); 
/* fprintfiüfpxyz,"^42d",runn); 
fprintf (fpe, "AZ2d",runn) و‎ 


fprintf (fpp,"“Z2d",runn); 
fprintf (fparam, "422d" ,runn? z 
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if (narray==0) 
€ 
fprintf(fpxyz,"As"," Single"); 
fprintf(fpe,"%s"," Single"); 
fprintf(fpp,"%s"," Single"); 
fprintf (fparam,"%s"," Single"); 
> 

else 
4 
fprintf (fpxyz,"4s"," Mult"); 
fprintf (fpe,"4s"," Mult"); 
fprintf(fpp,"%s"," Mult"); 
fprintf (fparam,"4s"," Mult"); 


+ */ 
fprintf(fparam,"\n Xs Z4e","CRITERIA - :",value); 
fprintf(fparam,"NnAs “Ze As Ze ^s Ae","xkkmiLO] = ", 

xkkmi1LO1,"  xkkmi1L1] 2 ",xkkml1EL11, 
" xkkm1L22 2ت‎ ",xkkm1L21); 
fprintf(fparam,"\n%s Ze 4s Xe","xkkmiL31] z ",xkkmiL3], 
" xkkmiL4] - ",xkkmi1L[413); 
fprintf(fparam,"NnAs Ae 4s Ae As Ae As Ae", 
FCO] = "FEO," FEI] = TEC, TEED] <= es 
rEt21," [E35] > ",,mnES325 
fprintf(fparam,"NnAs Ze As Ae 4s Ae","pkkmiLOl] = و"‎ 
pkkm1LO1," pkkm1Cid = ",pkkmiCid, 


| " pkkmi1L2] » ",pkkm1L[223); | 
fclose(fparam); 


fpinzfopen(í("trj.inp",'"w"); 


fprintf(fpin,"A4d A4d'",narrmray,runn); 

fprintf(fpin,"MnAe Ze 4e",xkkmliLOl,xkkmiL21, 
xkkm1L41)s/*positions*/ 

fprintf(fpin,"\n%Ze  Xe",xkkmiLil,xkkmi1L31)3:  /*vel*x/ 

fclose(fpin); 

if (optionzz4) goto quit; 


fptrj-2fopen("trj.dat","'"r"); 
fpzic-fopen("zic.dat","r"); 


waits: printf ("\nPress g and ENTER to start ... ۱ 4 
scanf ("%s" ,&c) ; 
lif (c!="’g’ && c!='G’) goto wait; 


if (nrange==0) 
< 
xmin=xminl; xmax=xmax1; 
ymin=yminl; ymax=ymaxl ; 
> 
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else 
£ 
xmax-xmaxhs;  xmin-2xminh; 
ymaxzymaxh: ymin=yminhs; 
> 


height=ymax-ymins 
width=xmax-xmins 
xscalezó600.0/width; 
yscalez-160.0/height; 
Dbyz196.0—ymin*yscaletOo.5; 
bx 2600, O-xmax #xscale+ôÔ. 3: 


biosini (6): 

frame (36,0,196, 600) s 

biosset (23,1); 
printf("A.lfZsA.lf",xmin,",",ymin); 
biosset (6,61): 
printf("A.1fAsA.lf",xmax,",",ymax); 
biosset (22,45); 

printf("Zs 4d","# ",runn)s 

biosset (23,65); 

if (narray==0) printf ("SINGLE"); 
else printf ("MULTIPLE"); */ 


printf("Nnwait و("‎ 


/ X جد اد پد‎ e جد‎ 9E 969€ 9E 3E 3€ 96 9€ 9E 903€ 909€ 909€ 9€ 96 96 3€ 9E 9€ 9€ 9€ 9€ 9€ 9€ 90 9€ 90 3€ 9€ 9€ 96 9€ 9€ 9€ 9€ 96 9€ 90 3€ 9E 9€ 9€ 9€ 9€ 9€9€ 909€ / 


/% */ 
/* KALMAN FILTER */ 
/* * / 


/ 3t 9€ 9€ 3E 9€ 3€ 9€ 3€ 9E 9€ 3€ 3€ 9 € 3€ جد جد جد جد 9€ جد‎ 9€ 9€ 9E 9E 9E 9E 3€ 3€ 9€ 3€ 9€ 9€ 9€ 9E 9E 9€ 96 9€ 9E E 9€ 9€ جد د جد‎ 9€ 96 96 90 9€ 9€ 9€ 9€ 9€ 90 9€ // 


/* START THE TIME SLOT LOOP AND SET ARRAY HANDOFF */ 
/* POINT */ 
for (kk=Oskk<itimes kk++) 


t 


dk=kk+13 


do 


t 


0051 2 )51 ۲6۵۱۸۳ 5 و ۳۱5۱ ۶1 و 5665 91 و ۲5 1 #1 و‎ 
hours=i1hours; mins=imins; secs=isecs; ths=iths; 
time=3600, *hours+é60. *mins+secs+0. O1*ths; 
deltatztime-oldtime; 
E 

while (deltat < 1.25 && kk»50); 


oldtime-time; 


if (deltat»1.51) printfí"s"); 
else printf("."); 
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/* 
/* 
/* 
/* 


adapt: 


printf("NnAs Ze","t = ",oldtime) 7۸ 
printt ("26 4d „" KK — 0“ +/ 
printf ("Nn 3 E E 3 € € € J 3€ JC C 9€ JEJE JEJE JE IEEE E E HF E E EE! ) 3 
printf("420s Xd ۲ 999 TIME :",kk); */ 


/* GET HYDROPHONE ARRAY COORDINATES */ 
if (narray != 0) 
{ 
if (xkkm1i[O] > swO) 18-05 
else if (xkkmi1LOl1 > swl) i8=12; 
else if (xkkmi1LOJ] > sw2) 18=24; 
else if (xkkmi1LO1 » sw3) i8=36; 
else if (xkkmi1LOlJ » sw4) 18-248; 
else 18-60: 
for (1=031<431++) 
{ 
j=3*i+i8; 
xbLil-hydroLj1; 
۷ ۲1 12۳۷۵۳۵۲ 7+1 [ 
zbLil-hydroLj-421; 
+ 
> 


/* GET THE TRUE TIMES AND THE TRUE POSITIONS */ 
fscanf(fptrj,"A41f AIF ۸۵1۴۳ و‎ 
و 2 ]212 50 و 1 1 ]2 31 20 و ۵1 ]۵ ۵1 له‎ 
fscanf (fezic, “4Zlf 21 "٣. 
&zicLOl,&zicL11,9*zicL21,&zicL351); 
truedLOol2dataro1; truedr11-2datar131; 
truedL212dataL21; 


/* FIRST GET HROW-CALCULATE GAIN, ESTIMATE */ 
/* COVARIANCE OF ERROR BASED ON ONE TIME */ 
/* MEASUREMENT-TC,TX,TY,TZ * / 
for (1=O;i<js;l++) 

í 

K=xkkm1COJ—-xbl1 ول‎ 

yzxkkmi[21-yb[1]: 

zzxkkmi[41-—zbrľi ]: 


denom=saqrt (x#x+y*y+z#z2) ; 
auxh=velm/denoms 
hrowLOlzx*auxh; 
hrowL21-2y*auxh; 
hrowL412z*auxh; 


for (11=03;11<nmax3iit+) 


i 
11n51i #nmax s 
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/% 


gnumCiij=pkkmiCiini*hrowlOJj+ 
pkkmiLtiin-c-2J*hrowL2J]-*pkkmliLiin*4J*hrowL41; 
> 
gdenom=hr owl O J#gnunl O J+ 
hrowl 2J#gnunl 2 J+ 
hrowl4J4#gnuml4J4rlid; 


/* THIS IS THE FIRST GAIN COLUMN د‎ 
for ČiirÓsii<nmax311++) gilii ]JzgnumL 11 ]/gdenom: 


printf ( "<nNVECTOR HROW =: \n")3 
printvecíhrow,3); 
printf("NnVECTOR GNUM : *n''); 
printvecígnum,9):; 
printf("NnDENOMINATOR 15 s: "): 
printf ("že \n",gdenom): 
printf("NnVECTOR GI : Nn"); 
printvec(gi,5); */ 


/% CALCULATE THE COVARIANCE OF ERROR PI */ 

for (11=03;11<nmax311+=2) 
4 : 
pdumLiil-í(-gitOlJ]x*xhrowLiil); 
pdumLii-51-2(-giL11]*hrowLiil); 
pdumLii-10]-2(-giL21*hrowLtiil); 
pdumLii-15J-2(-giL3J*hrowLiil); 
pdumL ii +20]2 (—g1i [4]#xhrowlii ]):s 
> 

pdum[0]+=1. 0; 

pdum[ 121+=1.0; 

pdum[24J+=1.0; 


p1LO1 szpdumL O]#pkkmi ľO]J+pdumL. 2] #pkkm1110]+ 
pdumL 41+pkkmi [20]: 

pil} J =pdunl[OJ#pkkmil1 J+pdunCl[2J#pkkniCll1l J+ 
pdumL4J*pkkm1L212J; 

-pdumLOl*pkkmiL2J*-*pdumL21*pkkmi1Li2l]-‏ 2 11م 
: [ 22 ۲ 1۸1 1*۵ 4 01:۲ 0 

+ [ ق 1 ] #۵۱۱61 [ 2 ]01:0 ۵+ ۰51 ] ۱۱۸1 1*۵ 0 ]01۸ << BILS]‏ 
و 1 23 ] #۵۱۱۸1 1 4 ]021۸ 9 

01] 4 [ <0 01۸۱] ۵ 1*۵ ۱۲61 ] 4 [+۵ 01۸6] 2 1 #۵۱1۸1 ]1 4 [ + 


pdumL41*pkkm1L241; 
8 1 ] ؟ 1 1] 1 1<0 ظ‎ 
۵2 ]61[ -pdumLSl*pkkml1Li1l]-*pkkmiLól- 
pdumL71*pkkmitill-pdumt9?1*pkkmirt211; 
0 1 ]7 1 -pdumLSl1*pkkm1L2J-*4pkkmi1t71- 
802۱۸0۲ 7 1*۵ ۱۱۸1 ] 1 ؟ [ 22 ]1 0۱1161* 1 9 ]01:0 ۵+ لد‎ 
piL8] -pdumLS1*pkkmi1L3l]*pkkmiL8l- 
pdumL71*xpkkmi1Lià3l-pdumL9?21*pkkm1L2351; 
piL9] -pdumL5l*pkkmiL41-pkkmit9?1- 
pdumL 7 ]#pkkm1114]+pdum. ?]#pkkm1i [24]: 
piL1012pit21; 
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/* 


01 ] 11 120 1 ]7 1 

Diti2jepdumt10J4Bkkmi E21 gÜIRLL21«phRmipi21m 
۳ 01:6] 1 4*0 ۲۲601] 22 ؟:‎ 

piti31-2-pdumL1Ol1*pkkmiL31-pdumLi12J*pkkmiti131- 
pdumC14]+pkkm1 23]: 

+143 11م اعام* 123 1ل ناك 5+ [ 4 1 1 مع !»ا م 103 1 مناك م2[ 4 1 1 1 م 
pdum—C14]+pkkm1 [24]:‏ 

piL15212piL31; 

۴ 1 ] 1 6 [<0 1 1 

pifi712piL[131; 

pitiglJ12pdumLiS21*pkkmiL31*pdumLi7J*pkkmiLi31- 
۵۱۲61 ] 1 91 +۳۴ 0۸] 1 9 [#0 ۱14601 ] 2 [ : 

piti912pdumLiS521*pkkmi1L41-pdumLi1721*pkkmi1L1431- 
pkkmiC1?]+pdunC 17?]+pkkm1[24]: 

piL2012pi1[41; 

pit2121-2piL9?21; 

pit22212piLt141; 

piL2312piL[191; 

Dit241J-pdumL201*pkkmi1LtA4J4pdumt2217*pkkm1t141- 
pdumL2431*pkkm1L2431; 


/* CALCULATE FIRST MEASUREMENT PREDICTION */ 
Zhat=velm*denom; 
zdiffCi lzzicli ]-zhat: 


/* COMPUTE THE GATE FOR ERRONEOUS TIME */ 
/* MEASUREMENTS */ 

pki-(pifOÓlJ > O) ? -piLOJ : pitO31; 
pk3z(piti21] < QO) ? -piELi123 : piL12]: 
۱52 61 ]24[ > OD) ? -piL241 : piL241; 
piis(pki » pk3) ? pkl : pk3; 

p = (pii > pkS) ? p11 : pkôs 


pgate-zsqrtíp)*velm;*/ 
x-p-*rcLtil; 
if (x 12 0.0) 
1 
if (x < 1.0) x=1.03 
do 


c 


و ات يرك 1ه 
K=( x + P/x ( +0.5:‏ 
temp-oldx-x;‏ 
rabs= (temp < O) ? -temp : temp:‏ 
+ 
while (rabs > 0,0):‏ 
+ 


pgate-x*velm; 
gate=3. O*pgate; 
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/* 


/* 


156: 


/* 
/% 
/* 
/* 
/* 


printf ("\nMATRIX PDUM : ۱۳ ؟‎ 
printmat (pdum,5,5)3 
printf("NnMATRIX PI : Nn"); 
printmat (pi,5,5);5 
printf("NnZDIFF is : و("‎ 

8۳۳2 ۲۱ ) ۱۸۵2۶ \n",zdifflid)s; 
printf("\nGATE 15 : و("‎ 
printf("\Ze ۱۳۳ 21691 4 */ 


/* EDIT INVALID TIME MEASUREMENTS */ 
zgate-(zdiffLi] « O) ? -zdiffLil =: 2zdifflids; 
if (zgate >= gate)? 
€ 
for (11=O3;1i<nmaxj3iitt+)? giLiil-O.0; 
zdiffLij]2999.0; 
/* TAG FOR INVALID TIME MEASUREMENT %/ 
> 


/* CALCULATE ESTIMATE BASED ON ONE MEASUREMENT%/ 
/* PREDICTION */ 
for (11=0O3;11<nmax311++) 

: 1 ۲1 1 1 < ۱۱61 ۲1 1 1+1 ۲1 1 1*2 01 + ۶] [ 


printf ("\nVECTOR XI : Nn"); 
printvecíxi,9); */ 


for (iizOsgii<nmaxzsii++) 

€ 

xkkmiLiil-xiLiil; 

pkkmiCiij=pilCiid; 

> 
for (jj=nmax3 jj<maxsq3 jjt+t+) pkkmiCjjl=piljijd; 
if (i == 3) goto 156; 


> /+ end JS loop %/ 


NOTE : CALLED ORIGINAL X(0/-1) ,XKKM1. UPDATED */ 
AFTER 1 MEASUREMENT CALLED IT XI, THEN MADE */ 
XKKM1 AND WENT THRU ITERATION AGAIN. AFTER */ 
YOU HAVE UPDATED XKKM1 FOR EACH MEASUREMENT */ 
XKK=XI AND PKK=PI. */ 


for (11=0311<nmax311++) 


t 
xkkliilzxiLii]: 
pkkCiiJ=piliil; 
} 


for (jj=nmax3 jJj<maxsq3 jj++) 


{ 
pkkL3j12piLjjl; 
> 
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printf ("\nUPDATING VECTORS INTERVAL ... Nn"); 
printf ("\nVECTOR XKKM1 =: Nn"); 
printvec(xkkm1,9); 

printf("NnMATRIX PKKM1 : Nn"); 

printmat (pkkm1,5,9) 3 


printf("NnVECTOR XKK : Nn"); 
printvecí(xkk,9); 
printf("NnMATRIX PKK : Nn"); 
printmati(pkk,9,9); */ 


/* RECALCULATE TIME MEASUREMENTS, and FORM */ 
/* ABSOLUTE VALUE OF RESIDUALS. */ 
for (1=03;1<431++) 


/* EDIT INVALID TIMES FOR ADAPTIVE MANEUVER */ 
/* ROUTINE */ 
E 
1+ )281 +11 < 999.0) 
{ 
K=xkkm1COJ]-xblid; 
yzxkkmliL21-ybLtil; 
z-xkkmi1L41-zbLil; 


/* denom=sqrt (x%x+y%*y+z*z); */ 
denom-x #xtyty +2 %2 s 
x-denom; 
if (x 2 0,0) 
i 
ift (x < 1.0) x=1.03 
do 


í 


و لات يباك 1ه 
x2( x + denom/x ( 0.5:‏ 
temp=oldx-x3‏ 
rabs=(temp > 0) ? -temp : temp;‏ 
;> 

while (rabs >= 1.e-12); 

>; 


denom-x; 
zhat=velm*denom; 
۲ 6660۳0 22 1 0]1 1-2۳3 : 
zdiffLi]z (temp < O) ^? -temp : temp; 
> 
else 
€ 
zdiffLilzO.0; 
nzdiff--1; 
> 


12 


/* THIS FUNCTION COMPUTES THE ADAPTIVE Q MATRIX */ 
if (icurve == 1) 
i 
bb2xkkmiLs3S]*xxkkmi1L31; 
0 0 <:4 ۲۱:۲1 ] 1 [ *: ۱۱6601 ] 1 [ 
al=bb+dd; 
bdzxkkmi1Li1l]x*xkkml1L31; 
el=dd*sigacc/al+bb*sigcc; 
el2=bd*sigacc/al-bd*sigcc; 
e2=bb*sigacc/alt+dd*sigcc; 
qlCOj=g2*el; 
qL112g3*el; 
qL212g2*e12; 
qL312g3*ei12; 
qLó612a2*e1; 
qL712q0E31; 
ه‎ ۲] 81 25 2*2 1 
qL1212g2*e2; 
qL1312g3*e2; 
qL1812a2*e2;; 
> 
else 
i 
bbzgammaLOJ]1*gammaLOo1; 
ddzgammaL1]#gammaL1]: 
qLoJsbb*covwLO1; 
qL1J2gammaLOl*covwLOl1*gammaL11; 
qL2120.0: qL 3120.0; 
qL61-2dd*covwLo21; 
617 1 20۰ ۵: q[8]=0. 0; 
6۲ 12 1 2*0۷ بب‎ ] 1 [ 
6۲ 1:5 1 29 ۵662] 01*0۷۷ ] 1 1*0 ۵0602 ۲ 1 1 : 
qrL18J2dd*covwL 11; 
> 
15131131: 1101-2123: ۲111207 ۶ 
1151-1351: 1151-8181: ۲17 120] 1.35 1 


/* printf("NnMATRIX Q s: Nn"); 
printmat (q,5,5)3 %/ 


/* IF ALL TIME MEASUREMENTS EXCEED GATE, BYPASS */ 
/* ADAPTIVE MANEUVER ROUTINE. */ 
if (nzdiff != 0.0) 


t 


2 01 ۶ 5۷< )2 01 + + ] 0 1+ 2013 +] 1 [+2 01 ۶+۲2 [+2 01 ۶ ] 5 1 ( ۸۲۳۱2 01 ۴+ : 
/% printf ("\nZDIFAV is : و("‎ 
printf ("\Ze \n",zdifav)5; %/ 
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/* 


/* 


/* DO NOT PERFORM ADAPTIVE MANEUVER IF FILTER */ 
/% HAS NOT ACHIEVED THE STEADY STATE. */ 
if (kk > 3) 

3 

ةِ ۶۳۱5۱ 51 و 5 566 91 و ۲۱5 12 51 و 5 0۱1۳ 51 ) 6 511 600 

hours=ihours; mins=imins$; 566 521 596 5 


ths=ithss 
times 3600, #hours+60. #nins+secs+O,O1+ths: 
printf("Xs Xe"," timel = ",time); */ 


/* IF ZDIFAV MEETS CRITERIA TRANSFER OUT ۷۸ 
/* OF ADAPTIVE MANEUVER ROUTINE. */ 
if (zdifav > value %% icont < maxopt) 
t 
/* INCREASE GAIN */ 
for (j=0O3 j<maxsq; j++) 
۵1۲۱8۸1 ] ز‎ 12۳11] 3 1+0] 3 ( + 


/* printf ("\n INCREASE GAIN AND PERFORM"); 
printf(" ADAPTIVE ... Nn"); 
printf ("\nMATRIX PKKMi ۶ Nn"); 
printmat (pkkm1,5,5)3 */ 


/* PERFORM ADAPTIVE MANEUVER BY */ 
/* REITERATING SAME TIME SLOT. */ 
1cont+=13 

goto adapt; 

+ 


>; 


1cont=0;5 
nzdiff=4; 
xdiffLOJ-xkkLOol1-truedELO1; 
xdiffL11]-2xkkE221—-truedEL11; 
xdiffL212xkkL41]—-truedL 21; 


temp-xdiffLi1l1*xdiffL11/pkkm1L121; 
if (temp > limvar) icurve-zO; 
else icurve-1; */ 


fprintf(fpe,"\n Ad Xe Xe “Ze",kk, 
7 02 ۴۴ ۲۷ 1 2 ۴۲ 1 ٩ xdi i Ds 


/% WRITE ۷۸ 

printf ("\nVECTOR ERROR : XDIFF YDIFF ZDIFF\n"); 
Printvec(xdiff,3)3 */ 

/* PREDICTIONS FOR PKKM1 ۸ 


for (1=O;i<nmax;i++) 
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/* 


/* 


/* 


1 

phipkkLiJl-2pkkLil-*pkkti-*5J1J*phisLOl; 
Phipkk£litSJ=pkklitS]; 
Phipkklit+lOJ=pkklit10OJ+pkklitidsi*phisl(2]); 
۵۳۱1 ۵11] 1 ۲ 1 5 [ 20 ۱1] 1 ۲1 5 [ و‎ 

و [ 24۵+ 1 ]1۱ 20 [ ۳20۵ ۵۲۲۲1 ۵۳۱1 

> 


for (i=O;i<nmax;i++) 
< 
in-i*nmax; 
pktempLinJ2sphipkktinl-*phipkkLin-*i1l1*phisLO2l; 
۵۱16600 ۲1 ۲+1 12۵۳۱3 ۵11] 1 ۲+1 [ 
و [ 2 )5 ۳۵۱۲۱1* [ ۳ 1] ۵۱۲۲ 1 1+۵ 0+2 1 ]۵۱1 28۳۲۱1 1 0۸۸۵ 1 ] ممع ها م‎ 
pktempLin-3l-phipkkLin-431; 
pktempLin-41-2phipkkLin-441; 
> 


for (j=0; j<maxsg; j++) pkkmiLjl-2pktempLji1-4qL731; 


printf("NnPREDICTIONS FOR PKKM1 ... Nn"); 
printf("NnMATRIX PKKM1 : Nn"); 
Printmat (pkkm1,5,9); */ 


/* CALCULATE NEW XKKM1 */ 
for (1120s511<nmax311++) 
t 
xkkmiLiil2xkkLiil; 
/* pi2sqrct(fabs(pkkLiil0); */ 
? 
xkkmiLOJsxkkmi ĽO]+phi sLOJ#xkkL1]: 
xkkmi1L2212xkkmiL21-*phisL221*xkkL31; 


printf("NnNEW XKKM1 ... (for next time"); 
printf(" interval)Nn"); 

printf("NnVECTOR XKKM1 : Nn“)s 
Printvec(xkkm1,5)5; */ 


fprintf(fpp,"\n Ad Xe “Ze Ze",kk, 
pkkELO3J,pkkE123,pkkL2413); 


fprintf (fpxyz,"\n Ad Ze Ze Ae",kk, 
XkkLO3J,xkkL2231,xkkL41); 


/* PLOTTING */ 
col=xkkCO1*xscale+bx; 
row=xkkC2J]*yscaletby; 
bioswd(row,col); 


biosset (3,0): 
printf ("4s 4A.lf","x z",xkkLO1); 
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quit: 


biosset (3,13); 

printf (isl 1" ير‎ "2 ID: 

biosset (3,25); 

printf ("4s A.1f","z2 =" XEREGIOS 

biosset (3,42); 

printf("AZs ۰ ۳ و‎ ۱ (0: 

biosset (3,54); 

printfo As Wl IT wv mu F L DS 

biosset (3,65): 

printf ("4s Zd","time =",kk)3; */ 


+  /* end TIME loop */ 


fclose(fpxyz); 
fclose(fpe); 
fclose(fpp); 
fclose(fpzic); 
fclose(fptr jj): 
c=keyrd(); 
biosini (3): 


printf (" Xngauit ? LY/NI] :"); 
scanf#("4s",%c)5 


e 


if (c==’ it C=="N’) goto restart; 


+ /* end MAIN PROGRAM */ 
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/ E 3€ 3€ 3€ 3€ 3E 3€ 3€ 3E 3E 3E E 3E 3E 3E 3€ 3€ 3€ 3€ 9€ JE JEJE E 3€ 3E E 3E JEJE 9€ 3E JE 3€ 3€ 9 E 9 € 9€ 9€ 9€ 9E 3€ 9E 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ € // 


/* 
/* 
/* 
/* 
/* 
/* 
/* 
/* 


PRINTVEC will print the vector elements given 
the name and the dimension. 


PRINTMAT will print the matrix elements in the 
usual matrix form, given the dimensions. 


*/ 
* / 
*/ 
* / 
*/ 
*/ 
*/ 
*/ 


US S K S ASUS Au Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su Su u Su Su Su € J€ 9€ 3€ € 9 9€ 3E 9€ € 96 9€ 9€ 9€ 9€ 90 9€ 3€ 9€ 909€ ہ۰‎ 


/* 


printvec (vector ,dim) 


int dims 
double vector[1; 


` 

int i, j; 

for (jsoOs j<dim; j++) printf("%e X1s",vectorLjl," 
2۳ 1 ۲۸ ۳ (UNPU) S 

> 


printmatí(matrix,row,col) 


int row,col; 
double matrixt1; 


< 
nmt 15.4: 
for (1=031<row31i++) 
< 
for (j=O03 j<cols3 j++) printf ("Ze 
A^1is",matrixLti*col-*jl," "); 
printf ("\n")5 
> 
+ */ 
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re 


/ 3€ 3€ 3€ 3€ 3€ 9€ 9€ 9€ 9€ 3€ 39€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 96 3€ 9€ 3€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ / 


/* */ 
/* FRAME */ 
/* Draw a frame on the screen. */ 
/* x** NOTE پر۶‎ */ 
/* The upper left corner of the screen is 0,0 and the */ 
/* bottom right corner is 199,639. */ 
/* */ 
/* */ 


/ 3*3t 3 9€ 969€ 9€ 9€ 9t 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 39€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ چو‎ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ / 


int frame(ulrow,ulcol,lrrow,lrcol?) 
int ulrow,ulcol,lrrow,lrcol; 

c 

int count,irow,i,iaux,icol,tcount; 


/* do top line first */ 
for (icol ~ ulcol; icol <= lrcol; 1 (++1هم‎ 
bioswd(ulrow,icol?); 


/* do both sides at once */ 
for (irow - ulrow*i; irow «X lrrow; irow-*-) 
3 

bioswd(irow,ulcol); 

و ( ۳061 1 و ۵۷۷ ۳ 1) ۵5۷۷0 1 ۵ 

J 


/* do bottom line */ 
for (icol = ulcol; icol <= Lrcols 1col++) 
bioswd(lrrow,icol); 


/* do vertical center line with scale */ 
i-20.9*(ulcol-*l1rcol)-40.35; 


for (irow = ulrow+l: irow < lrrows irow++)  bioswd(irow,1);j 
for (irow = ulrow+16; irow < lrrows irow+=14) 
for (tcountz-i-4; tcount <= 1+4: tcount++) 


bioswd (irow,tcount? ; 


/* do horizontal center line with scale */ 
i-O.9*(ulrow-lrrow)-40.35; 


for (icol -» ulcol-*1; icol «4 lrcol; icol*-*) bioswd(i,icol); 
for (icol 2 ulcol-60; icol < Ircols; icol+=60) 
for (tcountzi-3; tcount <= 125 tcount++) 


bioswd (tcount,icol); 


return (O); /* done */ 
+ 


148 


/ 3€ 3€ 9 3€ 9€ 9 9 9€ 3 9€ 3€ 3 3€ 3€ 3€ 3€ 3€ 3€ 3€ 9€ 9E 9€ 3€ 9€ 9€ 3€ 39€ 3€ 9€ 9€ 3€ 9€ 9€ 3€ 9€ 9€ € 9€ 9E 3€ 9€ 3€ 9€ 9E 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9 9€ 9€ 9€ 9€ 9€ 4 


/* 3 / 
/% TRAJECTORY SIMULATION ROUTINE - file: TR.C جد‎ / 
/* + / 


/ 3€ 3 3€ 3€ 3€ Je 3 3€ Je 9 3€ 3€ 3€ 9€ 9€ 3€ 9€ 9e 9€ 3€ 3€ 9€ 9C 3C 3E 3€ 9€ 3€ 9€ 9€ 3€ 9€ 3€ 3E 9€ 9 3C JE 3E 9 9€ 3E 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ € / 


#include "stdio.h" 
#include “math.h" 
#include "kalman.h" 
trajec (narray,hydro,xb,yb,2b,xkkm1, jstart, jend,wd,g,worg) 
0 و و ۷۵ و [ ] ۱۷61و ( ]29و ۷۳۲۲1 و 1 9۲و 1 ۲۱۷/۵۹۳۵۲ ۵ 1 باه‎ 5 
1 ۲۲ ۳۵۳۳۵۷ ۱۷۸۵۳3 و 5153۳1 3 و‎ [620 
Ç 
int pli,i,start,end; 
double ppl,r1,sqrt(),zcs,zic[41,ziL[41:; 
double xd,yd,zd,v,den,velm,gg; 
static double xn[6121-3.01, -—2.0, —1.5, 1.0, -0. 6, 
O. 035 
static double yn[6]={0.0, 0.0228, 0.0668, 0.1357, 
0927451 OF; 
static double sn[512143. 8596, 11.3636, 7.25689, 
2.891352, 2.458877; 


double 215 [ و‎ ۷۷۳ 
int runn,i8; f 
FILE *fptrj, #fopen(): 
FILE *fpzic, wfopen(); 


/* THIS ROUTINE COMPUTES TRUE TRAJECTORY OF TORPEDO 
datalOJ=true x position, datalljJ= true y position 
dataC2]=true 2 position, w=turn rate 
dataL312x velocity, dataL[41]-y velocity 
jstart=time slot where maneuver starts 
jend=time slot where maneuver ends */ 


0 51 5] 0 [ < ۱01 ۲41 ؟‎ dataLi1l-xkkm1L213: datalL[21J-2xkkmi1L41; 
data[3]zxkkmi1[1]: data[4]zxkkm1i [3]: velm21.0/vel: 
r1i=0.49; 

wr=twopitwd/360.03 gg=32.2%g3 start=jstart; end=jend; 


printf("\n “Zs \n", "start execution of trj.c"); 
fptrj=fopen ("trj.dat","w"); 
۴ 0 2 1 2<2۴ ۵6 ۲ )۲ 216۰ 0151 ۲۳ و ۲۷۷۳۲۸ و‎ 


for (33=03j33<npts jj++) 


i 
/* GET HYDROPHONE ARRAY CODRDINATES */ 
if (narray != O) 


1697 


چ / 


t 


if (dataLO] > swO) 18-0: 
else if (dataLOJ] > swi) 18-212: 
else if (dataLO] > sw2) 18-24: 
else if (dataLOJ] » sw3) 18=363 
else if (dataLO] » sw4) i8-248; 
else 18=60; 
for (1=031<431++) 
X 
j=3*1i+183 
xbLil-hydroLJj3; 
ybLil-hydroLj-*11; 
zbLil-shydroLij-*-21; 
> 
> 


/* COMPUTE THE TRUE TIMES AND THE TRUE POSITIONS 
WRITE TRUE TIME and TRUE POSITIONS */ 


for 


ra: 


Drimtto Ag LN 
fprintf (fptrj,"\n Xe Ze Ze", 
dataLOJ,dataL131,dataL21); 


) (ج+ج 1 1>4 و20‎ 
£ 
xdzdataL0]-xbrLi J; 
۷0 <0 ۵1 ۵] 1 حاب-[‎ ]) 1 ( 
2 020 32۲5۲21-2011 (5۶ 
den=saqrt (xd*xdtyd*yd+zd*zqd) ; 
ziLil-velm*den; 


printf ("\nTRUE TIMES : Nñ"); 
printvecízi,4); 

printf ("\nTRUE POSITIONS : Nn"): 
printvecí(data,3); */ 


/* GET NOISE,SCALE AND ADD TO TRUE */ 
/* MEASUREMENT TIME */ 
ppiz-2317.0*r1; 

pli=ppl; 

ri-ppi-pli; 

v=r 1; 

j20; 

if (v > 0.3) =l; O p ls 

if ۷ > ۷۳۱۲ +1 1۱ goto r8; 
Jj+=1; 

qoto r2; 

v—-(v-ynLjJo)*snL j1*xnL 323; 
if (rl >= 0.5) v=(-v); 
Zzce-(v/3.00)*0.00001; 


printf ("\nRANDOM NUMBER :");5 
printf ("Xe \n 26s). 7 
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zicLil-zcs-*ziLlil; 
> 
fprintf(fpzic,"Nn Ze Ze Xe Ze", 
2152190321 و رات تا 6 3 2 ولا لا 5 3 وا اج‎ 


/* NEW DATA */ 
if (start >= npt?) line(jj,9,npt,data) ; 
else 
{ 
if (end > npt) end=npt; 
line(jj,O0,start,data); 
circle(ij,start,end,data,wr,gg,worg); 
line(jj,end,npt,data?); 
> 
> 
fclose(fptrji); 
fclose(fpzic); 
> 


circle(j,start,end,data,ww,gg,worg) 
double dataL1,ww,gg; 
int j,start,end,worg; 
€ : 
double sin(),cos(),atan(); 
double r,vt,aux,a,Db,teta,tetaO,m,alfa,wa,ga,g,w; 
g-ggs WWW; 
1 ۶ ز)‎ >= start %% j «X end) 

f 


if (dataL3]1220.0 && data[41-220.0) goto out; 
vtzsgrt (dataL[ 3l#dataL[ 3]+data[141]+data[ 41): 
if (worg221) gzw#vts 
else w-g/vt; 
—vt/w; 
if (dataL31220.0) 
< 
b-dataL11; 
if (data[41>0.0) 


t 


a=datalOlj-r; 
۲ 61 .020و‎ 0: 
> 

else 


t 


zdatal0]+rs 
tetaO02z0. 5#twopi: 
^ 
> 
else if (data£l4]==0.0) 
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£ 
a-dataL[O1; 
if (datal 3120. 0) 
{ 
b=dataliil+r; 
tet aQ=—-0. 25*twopi; 
> 
else 


< 


bzdata(1]-rs 
tetaOzo.295*twopi; 
> 

+ 
else 

t 

m-data[41/dataL[ 31; 

aux zr /sgrt(1.0+m#m) s 

if (data(3] > 0.0) aux=—-aux; 

b=datalid-aux; 

a=datalOJ+m*(datalild—-b); 

۴۲ ۵1 و (1 4] 051 و 51 ]2 1 0-) 27۱2 ۵221 و‎ 
teta=1.3l#wttetad; 
datalCOJ=r*cos (teta) +a; 
datalilj=r*sin(teta)+b; 
data[3l1]2-vt*sin(teta); 
dataL[41-2vt*cos(teta); 
if (j==end-1) 


t 


wa-w*360.0/twopi; 

gazg/ 32. 25 

printf ("mn a.a.. from TRAJEC : "); 
6زم‎ 


printf('"NnZs Ze Zs"," turn rate : ",wa, 
" dg/seg"); 
printf( NnZzs ۷ءء‎ g-number : ",ga);; 
7 
outs 


+ 
+ 


line(j,start,end,data) 
double datal]: 
int j,start,end; 


£f 
Ú 


if((j >= start) && (j < end)) 
3 
dataL[Oj]*21.31*dataL[ 31; 
datalij+=1.3i*datal4]; 
> 


APPENDIX F 


LISTING OF CIEKR1.C - CIE VERSION 


/ 3€ JE JE Je 3 3e 3e JE 3€ He JE He 3E 3E 3E 3E 3E 3E 3E 3€ E E 9E 3E 3E 3E Je EHE HE HC DE 3C DE 3E JEJE DEDE HE 3E JEJE EE JEDE DE نچ دہ‎ HE 3E 3€ HE 3€ 969€ // 


/% */ 
/% HEADER FILE - KALMAN. H */ 
/% CIE MICROCOMPUTER VERSION جد‎ / 
/% */ 


C 3C 3E He dC eb deo deo Je Je OEC E UC JE HE UO HHC E Je He ACE C HC HE HE dE HC CAE HE HO HEU HO HE He R‏ یں 


define nmax 9 /* # of col. in pkk, q ... */ 

define maxsq 25 /* size of equiv. vector PKK,@...*/ 

#define js 4 /* it hydroph. in any hydrophone */ 
/* array */ 

define hcmax 12 /* # of col. in matrix HYDRO of */ 
/* hydroph. coord. */ 

define hmax 72 /* equiv. vector size of HYDRO */ 

#define dmax 3 

#define itime 150 /* max.# of time slots */ 

#define npt 130 

#define icmax 5 /* max.# of optimal iterations */ 
/* allowed */ 

#define criter 0,1 /* criteria to compare ZDIFAM */ 

#define na O /* may assume ONLY values O or 6 */ 
/* O 2»singles;ó-2» multiple arrays */ 

define xpos 10000.0 /* initial torpedo x-coordinate */ 

#define xvel - 350.0 /* initial torpedo x-velocity */ 

#define ypos 0.0 /* initial torpedo y-position */ 

#define yvel 5:0 /* initial torpedo y-velocity */ 

define zpos 0.0 /* initial torpedo z-position */ 

define swO  33OOO.0 |  /* swi define points where */ 

#define swl 27000.0 /% switching from one to */ 

#define sw2 21000.0 /* another */ 

tdefine sw3 15000.0 /% array %/ 

#define sw4 9000.0 /% is done. */ 

#define sigdiv 1.0 

#define scc 0.387463094 /* - in radians... * / 

#define sacc 34.2 

#define vel 4860.0 /* speed of sound N 

#define twopi “4.28315308 


#define 
#define 
#define 
#define 


#define 
#define 
#define 
#define 
#define 


rrvar 1.e-5 


rvar 


pkkdiag 1000.0 
limvar 


gmo 
gmi 
CWO 
يماع‎ 1 
22 


1.e-10 


1.5 


/* 


/* 
/% 


/* defines threshold to use Q*/ 


constant 


*/ 


initial was 1036.84 */ 
initial was 1026.84 */ 
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/ 3€ 3€ 3t 3€ 3€ 3€ 3 3€ 9€ 3€ 3€ J 3€ 3€ 9€ 3€ 3€ 3€ 9€ J€ 3C 3€ 3e 3€ E 9€ 9€ 9€ 9€ 3€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 3€ 9€ 3€ 3€ 3€ 963€ 3€ 96 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 


/* 3 / 
/* KALMAN FILTER PROGRAM FOR THE CIE MICRO */ 
/* (FILE . CIEKR1.C) */ 
/* * / 


/ 3€ JC 3€ 9€ 3t 3€ 3€ 9€ 3€ 3E 3E 3E 9E 9E 9€ 3E 9E 3€ 3E 3€ 90 9E 3E 9€ 3E 3€ 3€ 3€ 3€ 3€ 3€ 3€ 3€ 3€ 3€ JE 3€ 9E 9€ 3€ 9€ 9€ 9€ 9€ 9€ 90 9€ 9€ 9€ 9€ 9€ 9C 9€ 9€ 9€ 9€ 9€ 39€ ۰ 


#include "stdio.h" 

#include "math.h" 

#include "kalman.h" 

double 
xkkmi€nmaxl],pkkmiCmaxsqi,pkkUmaxsqi,xkk€nmax],dataldmax]; 


/* STARTING THE MAIN PROGRAM... */ 
main () 
{ 
FILE *fpxyz, *fopen(); 
FILE *fpe, *fopen(); 
FILE *fpp, *fopen(); 
FILE *fptrj3, *fopen(); 
FILE *fpzic, *fopen(); 
FILE *fparam, *fopen(); 
FILE *fpin, *fopen(); 
int ihours,imins,isecs,iths,col,row; 
int iB,i,j,kk,iin,icont,icurve,option,param; 
int trjopt,nrange; 
int ii,jJjJ,narray,nzdiff,in,pli,runn,c; 
int jstart,jend,worg; 
float rabs,zgate; | 
double height,width, ymax ,ymin,xmax ,xmin$ 
double xscale,yscale,bx,by; 
double hours,mins,secs,ths,time, oldtime, deltat; 
double sqrct(),oldx PE ns 
double denam. a lA adane E ا‎ 
double den,velm,temp; 
double a2,91,92,93,pi,xdiffL:31; 
double zhat,zcs,dk; 
double v,truedL[31,wd,g; 
double x,y,z,auxh,xd,yd,zd,sigcc,sigacc; 
double ai,ei,ei2,e2,bb,dd,bd; 
double pkl,pk3S,pk5,pgate,rgate,p,píl; 
double phipkkLmaxsqli,pktempLmaxsqi,qLmaxsq1; 
double pitmaxsqlJ,pdumLmaxsqi,qtempiLmaxsq1; 
double gnumtnmaxl,gilnmaxl,xilLlnmaxJ,hrowLnmax 1; 
double 1 5151م ,و [ 5 3 1 ” و [ 5 رز ] *# +01 2 و ل 35 ا 1 2 ,و ل 5 [ اع‎ 353 , 221351 5 
double xblCjsl,yblisl,zblisl,rrlCisi; 
double hydrotLhmaxd1,covwL31,gammabL 21,o0pt£[ 31; 


fpxyz=fopen("xkk.dat","w 
fpe=fopen("xdiff.dat","w 
fpp-fopen("pkk.dat","'w"); 


*/ 


/*® INITIALIZE CONSTANTS, HYDROPHONE MATRIX, PHI, 
TRAJECTORY, 


DATA FOR 
nzdiff=4; 
runn=13 narray=na; 
nrangezos 
jstart=40; 


hydroLol1236000.0; 
hydrol3J=36030.03 
hydroLól1236000.0; 
۲۱۷۵۳۵۲91 2356000۰ O; 


hydroL121230000.0; 
hydroL1251230030.0; 
hydroL181230000.0; 
hydroL211230000.0; 


hydroL2421224000.0; 
hydroL271224030.0; 
hydroL301224000.0; 
hydroL3531224000.0; 


hydroL361218000.0; 
۲۱۷۵۳۵۲59 1 21 80۵0۵۰ ۵ 
hydroL421218000.0; 
hydrol 451=18000.0; 


۳۱۷۵۳۵] 48 [ 2122000 5 
hydroL5121212030.0; 
hydroL541212000.0; 
hydroL9571212000.0; 


hydroL60126000.0; 
۲۱۱۷/۵۳ ۵1] 631 26030۰ 5 
hydroLó66126000.0; 
۳۱۷۸/۵۳۵۲ 569 1 2۵60000 ۰ 


و 2۳۱۷۵۳ [ ۵ ]۱ ۲۳ 


gammaLo12gmO; 
covwLOJ12cwO; 


3 000۰ 21 01 ] ۲۱61 
05 25۵ ات ] ۱۱01 ۸ 


for 
for 


for 


phistO]21. 31: 


icontzOo; 


jend=80 


۳ ] 1 [ <۳ ۳ج ب‎ ٣۶ 


xkkmiL112(—50.0)5 
xkkm114]1-0.05 


(izOsi<nmaxri++) 


phisfl11=0.0; 


AND LOAD 


icurvezi; 
par amzO; 


hydroL112 6000.0; 
hydroL412 6000.0; 
hydroL712» 6030.0; 
hydroL102126000.0; 


hydroL13126000.0; 
hydroL16126000.0; 
hydrot19126030.0; 
hydroL22126000. O; 


hydroL25126000.0; 
hydroL28126000.0; 
hydroL351126030.0; 
۲ ۷0۳۵] 54 [ 26000۰ 0۵ 5 


۲۷۵0۳۵] 57 1 26000۰ O; 
hydroL 40126000. 0; 
hydroL43126030.0; 
hydroL 46126000. 0; 


hydroL49226000.0; 
hydroL 52126000. 0; 
hydrolS551=6030.03 
۳۱۷۵0۳۵۲58 1[ 26000۰. 0 


hydrol611=6000.03 
۲۱۱۷/۵۳۵۲ 641 26000 ۰ 5 
hydroL[671256030.05 
hydroĽľ 70126000, 0: 


ri212rvar; 


gammal 1 1 29 1 
covwlij=cwl; 


(12031 <nmax31++) 
(3303 jánmax 3 j++) pkkmiLi+#nmax+j120. 05 
pkkmilLi #nmax+i J=pkkdiag; 


175 


X(O/-1), 


value=criter; 
۸/۲ 9 2 1 : 


8۲۱1 5121-21 . 31 5 


93750. 136: 


۳۱۷۵۳۵ ] 2 [ 
۳۱۷/۵۳ ۵۲5 1 < 
۳۱ ۷۵۳۵۲ 8 [ < 
۲۱۷0۳ ۵] 1 1 [ < 


hydroL1412 
hydroL 1712 
hydrot2012 
hydrot 2312 


۲۱۱۷/۵۳۵۲ 26 [ < 
۲۱۷۵۳ ۵۲29 [ < 
hydroľ 3215 
hydroL 353175 


hydroL381z 
hydroL411-2 
hydroL4412 
hydroL4712 


hydroL 501-2 
hydrot 531-2 
hydroL 561-2 
۳۱۷/۵۳ ۵۲559 [ < 


hydroLó21z 
۲۱۷/۵۳۵۲ 653 [ < 
۱۷/۵۳۵۲ 68 [> 
۳۱۷۵۳۵۲7 1 ز‎ > 


و ۵۳ 2۳۱۷ [ بت ] ۳ 


covwl2J=cw23 


xkkm1L22120.0; 


R, 


PO =1) 


wd=5. 730; 


sa s 


O O O O 


3 


"^B *aB “aa 


n as AB 


3 


OOOO OOOO OOOO 


OOOO‏ (ي) ن ن ني 


۹5 6 „as We we 


3 


O O O O 
O O O O 


3 


O O OO 
O Q O O 


3 


CI 
O O O O 
O O O O 


phis[3120.0; 


xbCOI=(R-15.0)3 ybCOIJ=(-15.0)3 zbľloOl2(-15.0)5s 
xb[1J= 15.0; YDC 1 m(—15.0): zb[1]=(-15.0); 
xbC2I]=(-15.0)3; ybC2I]= 15.0; zbl212(-15.0): 
xbCSI=(-15.0)3 ybC3I=(-15.0)3; zbC3J= 15.0; 


/% GET DATA INPUT ۸ 


9E 9E EE EE 9E 9E 9E 99€ /‏ جد جد جد جد جد جد جد جد KK 9E 9E‏ جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد 3E‏ / 


/% */ 
/* DATA INPUT ¥ / 
/* */ 


جب جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد د ر 


menus: /* OPTIONS MENU */ 
printf sn“): 
printf ("\n KALMAN FILTER "); 
۴66ج‎ ۰ 


printf ("\n 06 0 1 ٤ 

51 ۳۰۷۷۳۷۰۹۸۳۴ 

printf ("\n i) RUN TRAJECTORY"); 

6۴6 an ٤ 

printf ("\n 2) RUN KALMAN"); 

۳۳ 6 ("xn") ۶ 

printf ("\n 3) MODIFY PARAMETERS"); 
printf (n); 

printf ("\n 4) QUIT"); 


printf mn o 

if (paran! =0) 
i 
printf ("kn") s 
printf ("ná parameter was changed ... "): 
printf(" You may have to run"); 
printf(" TRAJECTORY before continue."); 
+ 

printf ("\nEnter value (1-4) 1: ول("‎ 

4 0۳0۱ 50611 و ۲ ۳۲۸۵۵ 5۳۱۴ 5۲ 

if (aption==1) 


X 


fpin-fopení('"trj.inp",'"w"); 

fprintf (fpin,"%d “~d" ,narray,runn) ; 

fprintf (fpin,"\n“ze Ae Ae", 
xkkmiLOJ,xkkmiLi1J],xkkm1L21); 

fprintf(fpin,"\n%e Xe",xkkmliL3J,xkkmiL41); 

fclose(fpin);j 


trajecí(narray,hydro,xb,yb,zb,xkkml1,jstart, jend,wd,g,worg) 5 
param-O; 
goto menu; 
D 
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else if (option==2) goto run; 
else if (option==3) goto modify; 
else if (option==4) goto quit; 
else goto menu; 


modify: 
printf ("\n MODIFY PARAMETERS OPTIONS :"), 
primtf( Nme»s 
printf ("\n 1) ZDIFAV stop optimization"); 
printf ("criteria"); 
printf ("\n 2) SINGLE / MULTIPLE ARRAY"); 
printf ("\n 3) INITIAL X, Y, and Z positions"); 
printf ("\n 4) INITIAL Vx and Vy velocities"); 
printf ("N\n 9) NOISE COVARIANCE — diag elem of“): 
printf ("R") s 
printf ("\n 6) ERROR COVARIANCE ¬ diag_elen of"); 


Prúmetr 6“ FRETS 

printf("Nn"); 

printf ("An 7) MAIN MENU — OPTIONS"): 
printf ("Nn"); 

printf("NnEnter value (1-7) : و("‎ 

scanf ("4d",&param) و‎ 


switch (param) 


€ 
case 1: 
printf("Nn Xs Ze", 
"Current CRITERIA is :",value); 
printf ("\nchange ? LY/NI 12"): 
scanf("4s",%c):5 
if (c=='n’ ff c=='’N’) break: 
‘printf ("\nEnter new value (real) : و("‎ 
scanf("Zlf",£value); 
break; 
case 2: 


printf ("\nSINGLE array is the default"); 
if (narray==0) 

printfí("NnZs","Current case is : SINGLE"); 
else 

printf ("\n“4s","Current case 15 : MULTIPLE"); 
printf ("\nchange ? LY/NJ :"); 
scanf("Z4s",&c)3; 
if (cz2-'n' ۲ ۱ cz22'N') break; 
printf ("NnEnter S for SINGLE or"); 
printf(" M for MULTIPLE array : "): 
scanf ("4s" ,&C) 5 
if (c==' s' i c= 
else if (c= 
else ۷ 
break; 


'8S') marray=0; 
| C22'M') narray=1; 


۰ 


O; 
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case 3: 

printf ("\nCurrent values are : و("‎ 

printf("NnAs Ze As Ze 4s ~e", "xpos z "„,xkkmiLOJ, 
" ypas > ",xkkmiL[2]," zpos = ",xkkmil4]);5 

printf ("<nchange ? [Y/N] :"); 

scanf ("%s" ,&C) § 

if (c=='’n’ i: c=='N’) breaks 

printf ("\nEnter new values : "); 

printf ("\n%Zs","xpos = و("‎ 

scanf("Xlf",&xkkm1LO2); 


printf("XnAs","ypos = و("‎ 
scanf ("Al1f",&xkkm1L223); 


printf("NnZAs","zpos = و(“‎ 
scanf ("“ZALF" ,&xkkm1l4]) و‎ 


break; 


case 4: 
printf ("\nCurrent values are 
printf("XnAs Ze 4s Ze", "xvel 

"  yvel = ",xkkm1L32); 

printf ("\nchange ? LY/NJ :"); 
scanfí('"As",&c); 
if (c==‘'n’ tt c=='N’) break: 
printf ("\nEnter new values : و("‎ 
printf ("\nZs","xvel = و("‎ 
scanf("A4lf",&xkkmi1L12); 


ms 


"axkkmiL[i1l,‏ ہے 


printf("XnAs","yvel = "); 
scanf ("41f",&xkkm1i[3]); 
breaks; 
case 9: 
printf ("\nCurrent values are : و("‎ 
printf“ 1nZs Ze 4s Ze 4s Ze As Ae","r[O] = و"‎ 
۳۲۷۵ 1 ۰" او ۱1و 2 ]يخ‎ ۳ ees و‎ 


۲۰.۳] bo: 
printf (" <nchange ? [Y/N] :"); 
scanf("Z4s",&c); 
if (c=='n’ fi c=='N’) break: 
printf ("\nEnter new values : "); 
printfí("NnZs","rLOl] - و("‎ 
scanf("Z1f",£&rLO23); 


printf("NnZs".Urr1J) - 0% 
scanf("ZX1f",8&r[1313); 


0٣ 06) ELT 


ll 
= 
< 

«s 
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scanf ("“2lF",&r0£2])3 


printfí("NnAs","rL3] = "); 
scanf("Z41f",&r L3); 


break; 

case ó: 
printf ("\nCurrent values are : "); 
printf("XnZ4s Ze 4s Ze Zs Ze","pkkmi£O] = و"‎ 


pkkm1LO32," pkkm1f£6] — ",pkkmilL6], 
"  pkkmif£€12] = ",pkkm1£124); 
printf ("JnZs Ze Zs Ze","pkkm1£18] = ",pkkmiCi81], 
" pkkmit241] z ",pkkm11241): 
printf ("<nehange ? LY/NI : ۲۱ 
scanf("Z4s",&c); 
if (c-2-'n' i! czz'N') break; 
printf ("\nEnter new values : ")5 
printf ("\nZs","pkkm1£0] = و("‎ 
scanf "A41f",&pkkm1L01); 


printf ("\n%s","pkkmif£46] = "); 
scanf ("Z2lF" , &pkkm1£6)); 


printf ("\nZs","pkkm1iCi2] = و("‎ 
scanf ("Z1F",&pkkm1£124); 


printf ("\n%s","pkkm1C18] 
scanf("Z41f",&pkkm1L18212); 


"s 


printf ("\n%Zs","pkkm1£24] 
scanf ("Z1F" ,&pkkmiC24]) : 


و(" 


break; 


case 7: 
goto menus 
break; 


default : 
break; 

D 

goto modify; 


run:  /* START EXECUTION */ 
prime? ("in )s 
printf ("\nEnter run d$ : '); 
scanf (" Ad" ,&runn?; 


fparam=fopen("param.dat","w");5 
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/* 


fprintf(fpxvz. Aza. runs 
fprintf(fpe,"A42d",runn?);j 
fprintf(fpp," A2d",runn); 
fprintf(fparam,' 'A2d",runn) ; 


if (narray==0) 
€ 
tprintf(fpxyýz. As 2 66 ٥٦ 
fprintf(fpe,"4s"," Single"); 
fprintf (fpp,"4s"," Single"); 
fprintf(fparam,"As"," Single"): 
> 

else 
£ 
fprintf(fpxýz. As“, — Mult); 
fprintf(fpe,"4s"," Mult"); 
tfprintfifppo «MU - 
fprintf (fparam,"4s"," Mult"); 
> */ 


fprintf(fparam,"\n Zs Ze", "CRITERIA » :",value); 
fprintf(fparam,"NnAs Ze As Ze Xs “Ze","xkkm1COJ = و"‎ 
xkkmiLO],“ xkkm1Cid -» ",xkkm1L1J, 
" xkkmi1L2] » ",xkkm1L231); 
fprintf (fparam,"\n4s Ze 4s Ae","xkkml1L35J] = ",xkkmil3il, 
" xkkm1[4] » ",xkkm1L[41); 
fprintf (fparam, "<nZs Ze 4s Ae 4s Ae As Ze", 
"rLOJ] = و۲۳۲3"‎ ELI — ٦ 
" rLiZ] 2 #1021." [6201120 clo. 
fprintf (fparam, "<nZs Ae As Ae As Ae","pkkmliL[O] 2 و"‎ 
pkkmi1LOJ," pkkmilCild = “,pkkmiCid, 
" pkkm102] = ",pkkm1[2]); 
fclose(fparam) ; 


fpin-fopen("trj.inp","w"); 


fprintf(fpin,"Ad A4d",narray,runn); 
fprintf (fpin,"\nže Ze Ze", 
xkkm1 CO], ,xkkm1iC2],xkkm1041)3/*positions*/ 
fprintf(fpin,"\nZe Ze", 
Kkkm101I1,xkkmiC31])3/*velocities*/ 
#fclose(fpin); 
if (option==4) goto quit; 


#ptrj=fopen("trj.dat","r" 
fpzic=fopen("zic.dat","r' 


/% INITIALIZATION FOR QFIND */ 
velm-1.0/vel; 

ali42phis[O1; 

aZ2-2al4*a14; 
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/* 


/* 
/% 


gi=a2*0.53 


g27gi*g1; 


g37ai14*g1; 
sigacc-sacc*sacc; 
sigcc-scc*scc; 
for (i=0;i<nmax; i++) 
for (j=0; j<nmax; j++) Qli*nmax+j]=0.0;3 
6۱63۵ 50-1 1251 01و 1 ۷*25 01 و‎ ۷ 


/* INITIALIZATION FOR HROW 7۸ 
hrowL1120.0; hrowL3120.0; 


/* INITIALIZATION FOR PDUM 7۸ 
for (11=0311<nmax3iit+) 
t 
jja^ii*nmax; 
pdumtL j j*11320.0; 
pdumL jj+312-0.0s 
> 
pdumLó5121.0; pdumL181z1.0; 


/* INITIALIZING RR */ 


for (11=0311<4siit+) rrLiil-2sqrtí(rtiil); 


printf("\nPress any key and ENTER to start ... '); 
scanf ("%s" ,&c) $ 


/* INSERT TIME CHECKING HERE */ 
/* START THE TIME SLOT LOOP AND SET ARRAY HANDOFF */ 


/* POINT */ 
for (kk=Oskk<itimes; kk++) 


£ 

print#("\n%s Ze","t = ۵10۲1 9( ۶ 
printf("Zs %d"," kk = ",kk); */ 
dk=kk+1; 


printf("Nnj3d 3 3 3 3 3 c 3 JC 9 JC 3C 3C JC 36 JC 36 3€ 3€ 36 3E 9€ 369€ 369€ 369€ ' ) 2 3e / 
printf('"420s Zd WMn","d4J3333** TIME :",kk); %/ 
printfí("Nnwait و("‎ 

۳۳ 111 5 


/* GET HYDROPHONE ARRAY COORDINATES ۷۸ 
if (narray != 0) 
< 
if (xkkmifOl] > swO) i8=0; 
else if (xkkmiLO] > swi) 1i18=12; 
else if (xkkm1ilOJ] > sw2) 18224: 
else if (xkkmiL[LO] » sw3) 18=36; 
else if (xkkmilO] > sw4) i8248; 


else 18=60; 

for (12031<4531++) 
€ 
j^3*i-c*i8; 
xbLil-2hydroLj1l; 
ybLil-hydroL;j-*11; 
zbľi ]lzhydroL j+2]: 
>+ 


> 
/* INSERT TIME CHCKING + WAITING LOOP HERE */ 


gettp: i 
/* GET THE TRUE TIMES AND THE TRUE POSITIONS 
WRITE TRUE TIME and TRUE POSITIONS */ 
fscanf(fptrj, "41+ 41+ 41+", 

80 31 21 ۵1 و 5053۲21۱ و 1 1 ]5055 و‎ 
fscanf (fpzic, "Alf Z1f lf ۸1 و‎ 

&21iclOl &2z2iclijl,&zicl2I1,%zicl3I);5 
truedLO]JzdataL 0]: truedL1]zdatal1]: 
truedL 21>datalL 2]: 


/* FIRST GET HROW-CALCULATE GAIN,ESTIMATE */ 
/* COVARIANCE OF ERROR BASED ON ONE TIME */ 
/* MEASUREMENT-TC,TX,TY,TZ */ 
adapt: for (12031< js31++) 

£ 

x2xkkmiLOl-xbEril; 

y-xkkmliL21-ybLil; 

z-xkkmiL41]-zbLi]l; 


denom=sart (x *x+y*y+z*z2) 5 
auxh=velm/denom; 
hrowLO]zx#auxh<s 
hrowL2]zytauxhs 

و ۳ با * 22 [ 4 ]۵۷۷ ۲۱۳ 


for (11=O3;ii<nmax3iit++) 
t 
iin-ii*nmax;j; 
gnumLiil-pkkmlLiinl*hrowtOl- 


pkkmifiin-*21*hrowL21-*pkkmiLiin-*4J]*hrowL41; 
> 


gdenom-zhrowLO3J*gnumtOJ-«-hrcowEL21*gnumE221-hrowEL43*gnumE41-*n7CLil; 


/% THIS IS THE FIRST GAIN COLUMN */ 
for (11=Osii<nmaxsiit++) giliiJ=gnumliil/gdenom; 


/* printf ("\nYECTOR HROW : Nn"); 


182 


printvec (hrow,5); 

printf ("\nVECTOR GNUM : \n")3 
printvecí(gnum,95); 

printf ("\nDENOMINATOR is : "2; 
printf ("Ze \n",gdenom) ; 

Printf ("\nVECTOR GI : Nn"); 
printvec (g1,53): */ 


/* CALCULATE THE COVARIANCE OF ERROR PI */ 

for (ii-O;iiínmax;ii-*-22) 
t 
pdumLiilz(-giL[OJ*hrowLiil); 
pdumLii-*51-2(-giLi12J*hrowLiil); 
pdum[ii-101-(-giL[21*hrowr[iil); 
pdumľii1+15]1- (—gi [3]l#+hrowlLii 1): 
pdumLii-*-201-2(-giL[4J*hrowL[iil); 
+ 

pdumLOJ-*21.0; 

pdumL122J-42z1.0; 

pduml24J+=1.0; 


81 ] 0 [ zpdumLOoJ*pkkmiLOl*pdumL22J*pkkmi1Li10OJ- 
pdum[41]+pkkm1 [20]: 

piL1J اس و رر دك‎ ١ em Z eic 016111 
pdumL42J*pkkm1L212; 

pil2] -pdumLO2J*pkkmi1L223-4-pdumL22J*pkkm1L12313- 
pdum[41+pkkm1l [22]: 

pil 3] -pdumLOoJ*pkkmiL3214*-pdumL22J*pkkm1L13J- 
pdumL4J*pkkmit231; 

piL41 zpdum[ O]J#pkkmi [4] +pdum[ 2] #pkkm1[141+ 
pdum[ 41#pkkm1[ 24]: 


piL512piL[13; 
81 ۲61 -pdumLSJ*pkkmiL[i1J-*pkkmiLól- 
pdum[ľ 7 ]I#pkkm1[11]+pdum[? ]#pkkm1 [21]: 
1173م‎ zpdum[5]#pkkm1 [21+pkkm1i [71+ 
ناكم‎ 1 7 1#pkkm1[121]+pdum[ ? ]J#pkkm1 [22]: 
pi[8] zpdunmľ[5]#pkkm1i [3]+pkkm1 [8]+ 
pdum[ 7 ]I#pkkm1i [13] +pdum[ ? J#pkkm1 023]: 
piL9J -pdumLSJ*pkkm1L[41-*pkkmiL9?1l1- 
pdumL721*pkkmiL[14J-pdumL93*pkkm1L[243; 
pi[10212piL21; 
81 ۲] 11 1<۳1 17 [ و‎ 
piC12]zpdum[ 10]#pkkm1 [2] +pdum[ 12]+pkkm1[12]+ 
pdum—[ 14]+pkkm1 [22]: 
pi[13]zpdumľ 10] #pkkm1 [3] +pdum[ 121+pkkm1[(13]1+ 
pdumL14J*pkkm1t231; 
pi[142]zpdum[—[ 10] #pkkm1 [41]+pdum[— 12]+pkkm1[14]+ 
pdum[141*pkkm1L241; 
8 1 ۲] 151 <21 (ت]‎ 
0 1 ] 1 6 1<۳61 ] 98 [ 
piLt171-2pir[131; 
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/* 


pi[18]zpdun— 15]+pkkmi CL Ž1l+pdum—C17]+pkkm1[13]1+ 
okkm10181+pduml19I1*pkkm1C23)]; 

ج+ [ 114 ۳۱101* 1 17 ]01۸ ۳ج 1 4 ] ۱۱61 1*۵ 15 0۱:6۲ 20 [ 9 1 ۲ ۲1 
: 241 ]۵۱۱61 [ 19 ]01:6 ۵+ [ 19 ]۲ 1 معاعا م 

pir20]zpi[24]: 

piL2112piL91; 

piL2212piL141; 

81 ] 2.5 1<01 ] 19 1 

ج+ [ 14] 0۱۱61 * [ 01۸۲222 ۵+[ 4 ] ۱۱61 1*۵ 20 01:61 ۶۵ [ ۲24 1 ۵ 
ی۶ 241] #۵۱661 1 24 0۱:۲ 8 


/* CALCULATE FIRST MEASUREMENT PREDICTION ۷۸ 
zhat=velm*denom; 
zdifflid=zicliij-zhat; 


/* COMPUTE THE GATE FOR ERRONEOUS TIME */ 
/* MEASUREMENTS */ 

pki-(piLOol] > OD) ? -piLOlJ : piLOoO3; 

8۱32 )۳81 ۲1 21 > O) ? -pst£12J] + ۶ 
pk92(p1L24] X O) ? -piL24] : pir241; 
pii=(pki > pk3) ? pk1 : pk3: 

p = 4۳11 > pkS) ? pii : 


pgatezsgrt (p) *velms */ 


وم ي 
if (x != 0.0)‏ 
x‏ 
if (x < 1.0) x=1.03‏ 
do‏ 
< 
Oldx=x;‏ 
x=( x + p/x )*O.9:‏ 
temp=oldx-x;‏ 
rabs-(temp X O) ? -temp : temp;‏ 
J‏ 
while (rabs > 0.0);‏ 
> 


pgate=x#vel n; 
gate=3.0*(pgatetrrf[id); 


printf("NnMATRIX PDUM : Nn"); 
printmat(pdum,23,2); 
printf("NnMATRIX PI : Nn"); 
8۳ 1۲۱۴6۵1 )۳3 و (9 و9 و‎ 

printf("NnZDIFF is « “j; 
printf ("NAE ۳ (2 01 ۲ ۰ ال‎ 
Printf("\nGATE 15 : و("‎ 
printf ("\Ze \n",gate); */ 
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/* 


156: 


/* EDIT INVALID TIME MEASUREMENTS */ 
zgate-(zdiffLil < O) ? -zdiffLil : zdiffLtii; 


if (zgate >= gate) 
< 
for (ii=O;ii<nmax;ii++) 91111 1-0. 0 
2 01 ۴۴ 1 [ 2999 0 
/% TAG FOR INVALID TIME MEASUREMENT ۸ 
>+ 


/* CALCULATE ESTIMATE BASED ON ONE */ 

/* MEASUREMENT PREDICTION 7۸ 

for (11=0311<nmax31i++) 
xiLiil2xkkmiLiil-*giLiil*zdiffLil; 


printf © \nVeEGTOR AL 7777 
printvecíxi,3); */ 


for (11=0311<nmax3iitt) 


t 


xkkmiLiil-xiLiil; 

pkkmiLiil-2pitiil; 

> 
for (jj5>nmax sz ji<maxsag3 jj++) pkkmiC jj]lzpiC jj]: 
if (i == 3) goto 154; 


+> /* end JS loop */ 


/* NOTE : CALLED ORIGINAL X(0/-—1) ,XKKM1. */ 
/* UPDATED AFTER 1 MEASUREMENT CALLED IT XI,*/ 
/%* THEN MADE XKKM1 AND WENT THRU ITERATION  */ 
/* AGAIN. AFTER YOU HAVE UPDATED XKKM1 FOR */ 
/* EACH MEASUREMENT XKK=XI AND PKK=PI. * / 
for (1120s3511<nmaxz3i11++) 

1 

xkkLiilzxiLtiil; 

8۲۲] 1 1 1201 ۲1 1 ( 

I 
for (jj2»nmax;jjXmaxsqs jj-*-*) 

€ 

pkkE3312piLjJj1; 

+ 


printf ("\nUPDATING VECTORS INTERVAL ... \n")s3 
printf ("\nVECTOR XKKM1 : Nn"); 
Printvec(xkkm1,5)5 

printf (" <nMATRIX PKKM1 : Nn"); 

Printmat (pkkm1,5,9) و‎ 


Printf ("\nVECTOR XKK =: Nn"); 


printvecíxkk,9)1: 
printf("NnMATRIX PKK : Nn"); 
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printmat (pkk,5,5)5 ` */ 


/* RECALCULATE TIME MEASUREMENTS, and FORM */ 
/* ABSOLUTE VALUE OF RESIDUALS. */ 
for (12031<431++) 


/* EDIT INVALID TIMES FOR ADAPTIVE MANEUVER */ 
/* ROUTINE */ 


t 


if (zdifftil «X 999.0) 
€ 
x-xkkmliLOl-xbLil; 
zxkkmi[21-ybľi]: 
zzxkkm1[41-zbLi]: 


/* denom-sqrtiüxsxty*ytz*z)s5 */ 


denomasx*x-ty*ytz*z; 


x-denom; 
if (x != O.0) 
{ 
if (x < 1.0) x=1.0; 
do 
i 
oldx-x; 


xX-( x * denom/x )*0.5; 
temp=oldx-x3 
rabs-(temp < O) ? -temp : temp; 
? 

while (rabs >= 1.e-12); 

I 


denom-x; 
zhat=velm*denom; 
۲ 606 22 1 2]1 1-2۳3 و‎ 
موه غ ) >[ 1 ] + + 01ج‎ > O) ? -temp : temp: 
> 
alse 


t 


zdiffLi 150.0: 
nzdiff—-=1; 
> 

>+ 


/* THIS FUNCTION COMPUTES THE ADAPTIVE Q MATRIX */ 
if (icurve == 1) 

t 

bb-zxkkmiLšl#xkkm1i[ 3]: 

و ( 1 ] #۱۱661 [ 1 ] 1 1۱420 :<< 0 0 

al=bb+dd;5 
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/* 


bdzxkkmlLi2ls*xkkmiL31; 
el-dd*sigacc/al-*bb*sigcc; 
el2-bd*sigacc/al-bd*sigcc; 
e2-bb*sigacc/ai-*dd*sigcc; 
qLOo12g2*e1; 

qL1J2g3*el; 

qL2129g2*e12; 

qL312g3*e12; 

qLó612a2*e1l; 

qL7212qE31; 

qL8212a2*e12: 

qL12212g2*e2;; 

qL131]2g3*e2; 

qL18212a2*e2; 

> 


else 


t 

bb-gammaLOJl*gammaLO23; 
ddzgammaL 1 ]#gammaL1]: 
qLOJ2sbb*covwLOo1l1; | 
qlij=gammalOl]*covwlOl*gammalid; 

6121 20۰ ۵: gf31=0.0; 

qLó6J2dd*covwLO1; 

qL7120.03 qL812060.0; 
qL12]-bb*covwL11; 

6٩۲ 1 ۰5 [ <9 200051] 0 1*20 ] 1 [ * و‎ ۵۵] 1 [ 

ء [ 1 ]0*0۷ 0< [ 9 1 ]6 

> 

151-1133 qLióoóJ2qL22: 6۲11 207 1 
qLi5225qL31; qEL1ó6122qr823; ۲17 1201 


printf ("\nMATRIX @ z Nn"); 
printmat (q,5,5) و‎ */ 


/* IF GLL TIME MEASUREMENTS EXCEED GATE, BYPASS */ 


ADAPTIVE MANEUVER ROUTINE. */ 
(nzdiff !2 O.0) 


t 


zdifav=(2difflol+zdifflilt+zdiffl2]+zdiffl3]) /nzdiff; 


/* 


printf("NnZDIFAV is : و("‎ 
printf ("Ze Mn",zdifav): */ 


/* DO NOT PERFORM ADAPTIVE MANEUVER IF */ 
/* FILTER HAS NOT ACHIEVED THE STEADY STATE. */ 
if (kk > 3) 


t 


/* IF ZDIFAM MEETS CRITERIA TRANSFER QUT /* 
/* OF ADAPTIVE MANEUVER ROUTINE. * / 
if (zdifav > value && icont < 5) 
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/* 


/* 


*/ 


skip: 


í 
/* INCREASE GAIN */ 
for (j=0; j<maxsq; j++) 
pkkmiL j]lzpkkC j1+a[—L jJ: 
printf ("\nINCREASE GAIN AND PERFORM"); 
printf(" ADAPTIVE ... \n")3 
printf ("\nMATRIX PKKM1 : \n"); 
printmat (pkkm1,5,5)3 */ 


/* PERFORM ADAPTIVE MANEUVER BY */ 
/* REITERATING SAME TIME 
SLOT. 


icont-*tz1; 
goto adapt; 
> 


+ 


icont-zO; 
nzdiff=4; 
xdiffLOJzxkkLOJ-truedL[O1l1; 
xdiffLi1J]-xkkL2212—-truedL[11J; 
۲ 01 ۴ ۶ ] 2 [ < ۲۷۱۷] 4 1-1۳۵0] 2 [ 


۲ 500 2 01 ۶ ۶ ] 1 [ *( 01 ۴ ۶ ] 1 [۸/۱1۸1 ] 1 2 [ و‎ 
if (temp < limvar) icurve=0; 
else icurve=1; */ 


fprintf(fpe, "Jn Zd Ze “Ze Ze", 
۱۷۲ و5 1۱ 2 ] ۶ ۶ 01و [ ۴۴۱1 01و ۵1 ] ۶ ۶ #01 و‎ 


/* WRITE ۸ 
orintf ("\nVECTOR ERROR s XDIFF ۲۳۳ بطم‎ ۱ FF 
printvec(xdiff,3)3 */ 


/* PREDICTIONS FOR PKKM1 7۸ 


for (i=O;1<nmax;i++) 
í 
phipkkLiJ-2pkktil-pkkLti-S51*phisLtOol; 
PhipkklitSJ=pkkli+5]; 
pPhipkklitlOJ=pkklitlOl+pkklit+1iSI*phisl2]I; 
phipkkLi-i15]2pkki-151; 
phipkklit+20J=pkkli+20); 
} 


for (i=0;i<nmax; i++) 
{ 
in-i*nmax; 
pktempLinl-phipkkLinl-*phipkktin-*1J*phistOl; 
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/* 


/* 


mult: 


/* 


pktemp[in+1J=phipkk[in+11l; 
pktempLin-*212phipkkLin-*231-*phipkkLin*3l*phisL[21; 
pktempLin-3l-phipkkLin-c-31; 
pktemplint+4J]=phipkklint+4]; 

> 


for (j=03 j<maxsq;3 j++) pkkmilCjl=pktempl[jj+q[j]; 


printf ("\nPREDICTIONS FOR PKKM1 ... Nn"); 
printf ("\nMATRIX PKKMi : Nn"); 
printmat (pkkm1,5,9); */ 


/* CALCULATE NEW XKKM1 %/ 
for (11=O03;ii<nmax3iitt) 


t 


xkkmiLiiJ-2xkkLiil; 
/* pli-sqrtí(fabs(pkkLiil)0)3 */ 
> 
2 ۱6۲6۸1 ] 0 [ هر‎ ۲6۰ 1 ] ۵ +۵۳۱1 5 ] ۵ [ * ۲۲6] 1 [ 
7 ۲۲۰۸1 ]  [ <« ۲8۸1 1 2 1 +۳۱۱1 5 ] 2 [ *( ۱۱ ] : 


printf ("\nNEW XKKM1 ... (for use in next time"); 
printf(" interval) \n"); 

printf ("\nVECTOR XKKM1 ۶ Mn"); 
printvec(xkkm1,5)3 ۷ 


fprintf(fpp,"\n Ad Ze Ze Ze", 
kk ,pkkLO3,pkkL121,pkkL2413); 


fprintf (fpxyz,"\n Zd Ze Ze Ze", 
kk ,xkkLOJ,xkkL22,xkkL 43); 


+> /* end TIME loop */ 


fclose(fpxyz); 

fclose(fpe); 

fclose(fpp); 

fclose(fpzic); 

fclose(fptrj); 

printf ("press a key and ENTER to continue ..."); 
56 5۳۱+ )۲۸ 5 و 856۱ و‎ 


printf ("\nquit ? [Y/N] :"); 
scanf ("“Zs",&c) $ 
if (e=='n’ 11 << ۲ ( goto menu; 


+> /* end MAIN PROGRAM */ 
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printvec (vector ,dim) 
int dim; 
double vector (id; 


t 

int i; j; ۱ 

for (js0;j«dim; j-*-*) printf("Xe 15۳ ۷۵۲0۳] 3 و[‎ " "( 
printf ( 1m ۸ 5 

> 


printmat (matrix ,row,col) 
int row,col; 
double matrixt31i; 


i 
int 1,3; 
for 120۵5 1 >۳ و094‎ 1 ++ ( 
4 
for (1503 j<col; j++) printf ("Ze 
Zis".matrixLiscoglegd. ME 
printf("n"); 
>+ 
۳ FF 
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trajecí(narray,hydro,xb,yb,zb,xkkmi, jstart, jend,wd,g,worg? 


/* 


double hydroli,xbli,yb£lI,zbCI,xkkmiCi,wd,g;3 

int narray,worg,jstart,jend; 

£ 

int pii,i,start,end; 

double ppi,ri,sqrt(),zcs,zicL[41,ziL[41; 

double xd,yd,zd,v,den,velm,gg; 

static double xn[61={-3.01, -2.0, —-1.5, 1.90, —0.6, 
0.035 

static double ynL[ó612£0.0, 0.0228, 0.04668, 0.1357, 
0.2743, 0.533 

static double sn[512(43.8596, 11.3636, 7.25489, 

2.891552 2 535255 7: 


:خم و 033153 double‏ 

int ۳۱۲۱۲۱ 8٤8 

FILE *fptrj, *fopen(); 
FILE *fpzic, *fopen(); 


THIS ROUTINE COMPUTES TRUE TRAJECTORY OF TORPEDO 
datalOl=true x position, data[il1-2 true y position 
datal21=true z position, w=turn rate 

dataL312x velocity, dataL[41-2y velocity 

jstart=time slot where maneuver starts 
jend=time slot where maneuver ends */ 


dataLOl1zsxkkmiLO1s 0515 ۲ 1 ( < ۱۱01 ]2 1۶ 031] 2 [ < ۱۲01 ]4 1 
dataL312xkkmiLild; 0115] 41 211۲001 ] 51 velm=1.0/vel; 
ri=0. 49; | 
wr-twopi*wd/360.0; gg=32.2%g3 start=jstart; end=jend; 


printf ("\n Xs \n", "start execution of trj.c"); 
#ptrj=fopen("trj.dat","w"); 
fpzic=fopen("Zzic.dat","w"); 


for (jj=03 3541505 3 j++) 


< 
/* GET HYDROPHONE ARRAY COORDINATES */ 
if (narray != O) 


£ 


if (dataL[0] > swO) 1820; 
else if (datalO] > sw1) 18212: 
else if (dataL0] > sw2) 18-24: 
else if (datalO] > sw3) 18-36: 
else if (datalO] » sw4) 18248; 
else 18=60; 
for (1=031<431++) 

i 

j^S3S*i-*i8; 

xbLil-hydroL[jil; 
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۷ ]1 12۳۷70۳ 0۲*1 1 
zbLilshydrotj-*21; 
+ 
} 
/* COMPUTE THE TRUE TIMES AND THE TRUE POSITIONS 
WRITE TRUE TIME and TRUE POSITIONS */ 
printf("Zs",".,")5 
fprintf(fptrj,"n 4e 4e Ze", 
datalOl],datalli,datal2]); 


for (1203 1<4453i++) 
xd=datalOl—-xblids 
ydzdata(1]1-yb([i J; 
zd-dataLZl-zbtLtil; 
den=sort (xd*xdtyd*ydtzd*zd) ; 
ziLil-2velm*den; 


/* printf ("\nTRUE TIMES : Mn"); 
printvecí(zi,4); 
printf("NnTRUE POSITIONS : Mn"); 
printvec(data,3)3; %/ 


/% GET NOISE,SCALE AND ADD TO TRUE */ 
/* MEASUREMENT TIME */ 
ppiz317.0*r1; 
pli-ppi; 
ri-ppi-pli; 
v-r1; 
350: 
if (v > 0.0! v-1.0-ri: 
r2: 1* (v 1 ýnr7F511) gota ma: 
j+=1; 
goto rzs 
r8: v-2(v-yntLj2»)*snL j21*xnL j1;j 
if (ri >= 0.3) vza(-v)5 


2cs=(v/3.00) #0. 000013 


/* printf (" 4 RANDOM NUMBER :"); 
printf ("Ze \n",2c05) 5%*/ 
21231 12205 +211 js: 
> 
fprintf(fpzic,"Mn Xe Xe Xe Xe", 
zicLOJ,zicLlJ,2ieD 292360 2305 


/% NEW DATA */ 
line(jj,O0,150,data); 
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fclose(fptr ji): 
fclose(fpzic); 
> 


line(j,start,end,data) 
double dataL1; 
int j,start,end; 


3 

if((j >= start) && (j «X end)? 
{ 
dataLO1t*z1.31*dataL351; 
dataL11-*-1.31*dataL41; 
> 

> 
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APPENDIX G 


LISTING OF PLOT.C - PLOTTING ROUTINE 


/ Xt 3€ 9€ 3€ 3€ 9€ 3e 9€ 3€ 3€ 3€ 3€ 3€ 3€ 9€ ل‎ 39€ 3€ 3€ 9€ 9€ 9€ 9€ 9E 9€ 3€ 9€ 9€ 3€ 9€ 9€ 9€ 9€ جد‎ 9€ 3€ 3€ 3€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 3€ 3€ 9€ 9€ 3€ 3€ 3€ 9€ 9€ 9€ / 


/* | i */ 
/* PLOTTING ROUTINE - FILE : PLOT.C (IBM-PC) * / 
/* */ 


/ 3€ 3€ 3€ 9€ 3€ 3e 3€ 3€ 3€ 3€ 9€ 9€ 3€ 3€ 9€ 3€ 3€ 9€ 9€ 9€ 9E 9€ 9€ 9€ 3€ 9€ 9€ 9€ 3€ 9€ 9€ 3€ 3€ 9 3€ 9€ 3€ 9€ 9€ 3€ 3€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 3€ 39€ 39€ / 


#include "stdio.h" 
#include "math.h" 
#include "limits.h" 
#include "kalman.h" 
main () 
c 
float ixLnptl,varLnpt31,sum,sumsq; 
int i,col,row,inc,c,tt,choice,narray,runn,op,cc; 
float xLnptJ, yl€npti, zLnptJ,tLinptd1,xx, yy, zz; 
float xmax,xmin,ymax,ymin,poOwer,yaux; 
float height,width,xscale,yscale; 
float ymaxt,ymint,xmint,xmaxt; 
double xkkm1[5]: 


FILE *fpt, *fopen();j 
FILE *#foxyz, *fopen (); 
FILE *fpe, *fopen(); 
FILE *fpp, *fopen();j 
FILE *fpin, *fopen(); 


fpinzfopen("trj.inp","r"); 

fscanf (fpin,"4d Xd",£&narray,&runn); 

fscanf(fpin,"AlF AIF Alf Alf 41+", 
&xkkmiLOl,*txkkmiL22,9?xkkmiL42,2xkkmiLll, 
&xkkmi1L31); 

fclose(fpin); 


CLR. SCRN; 

printf ("\n INITIAL CONDITIONS FOR RUN # "); 
printf ("Zd",runn)s; 

puts("Nn Nn"); 

if (narray==0) 


printf ("\n CASE is SINGLE ARRAY.")1 
else printf ("\n CASE 1s MULTIPLE ARRAY."); 
printf("\n ~s Xe Xs he. 4s Ze \n"," xpos = ", 
xkkmiLOÓJ," ypos = ",xkkmiC2]," zpos = ", 
xkkm1l4]) 5 
printf ("\n Xs Ze 4s Xe \n"," xvel = ", 
*kkm1L12," yvel = ",xkkmiC3]); 


choice=1; 
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mop: 


printf (“"\n\n\n\n\n\n\n\n\n\n\nPress any key to"); 
printf(" continue ... "); 
c=keyrd(); j 
«or ( "9 

< 
CLR_SCRN; 
printf (" PLOT OPTIONS =: "2; 
Seincr (Nn )3 
printf ("\n 1) X EST vs TIME SLOT  '); 
printf ("\n 2) VEST VS TIME SEgT 9): 
printf ("\n 3) Z EST vs TIME SLOT  "); 
printf ("An 4) YoEST veex EST"); 
printf ("An 9) X ERR vs TIME SLOT  "); 
printf ("\n 6) Y ERR vs TIME SLOT  "); 
printf ("\n 7) Z ERR vs TIME SLOT  "); 
printf ("\n 8) X VAR vs TIME SLOT و("‎ 
printf ("An 9) Y VAR vs TIME SLOT و("‎ 
printf ("\n 10) Z VAR vs TIME SLOT  "); 
printf ("\n 11) PKK_X vs TIME SLOT "); 
printf ("\n 12) PKK_Y vs TIME_SLOT  "); 
printf ("\n 139 PKK Z vs TIME SLOT و("‎ 
printf ("\n") 5 
printf ("\n 14) TRUE TRAJECTORY  "): 
printf ("\n 19) CHANGE PLOT Ľdot <-> linear“): 
printf“ (default=linear) I"); 
printf ("\n 14) EXIT ")3 
BFINE"; ۱ 
printf “An enter value (1-16) : "); 
CUR, SAVE; 
scanf("A4Zd",&op); 
if (op«1 i! op»16) 

€ 

BELL; 

CUR. REST; 

goto mop; 

y 
printf("NnZsAdAs",'" option is ",op, 


os VEY INA ie 

scanft('Zs",&cc);j 
if (cc!2'y' && cc!2'Y') 

< 

BELL: 

CUR. REST; 

goto mop; 

> 
printf ("\n wait  "); 


Switch (op) 


€ 
case 1: 
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í 

fpxyz-ftopen("xkk.dat","r"); 

fscanf (fpxyz,"\n Ad Af Zf 419" BEE XX VY و‎ 52 2 ( ۶ 
xLOlJ2xx; . ۱012 و‎ 

ymax=xx3 ymin=xx3; xmax=npt; xmin=0.03 

for (i=lsi<npt3;it++) i 


fscanf(fpxyz,"Nn Ad Zf ZF ZF", 
51 و 50/۷ و 544 و‎ 52 2 ۱ 5 
xLil-xx;  tLil-tt; 


if (xx^ymax) ymax^xx; 
else if (xx<ymin) ymin=xx3 


۳۱۳ ۱۵۳ ۰ 425 “4 1s 

> 
plot(ymax,xmax,xmin,ymin,x,t,choice); 
biosset (3,30); 
printf(" Zs ","X EST vs ٣6 is 
biosset (4,8); 
printf( 45. ۰۳ ۱ 6 
+ وه 1ج‎ )+ 0۷2 ۱ : 
break; 
> 

case 2: 

t 
fpxyz-ftopen("xkk.dat","r"); 
fscanf(fpxyz,"Nn Ad Af Af Af'",&tt,S&xx, &yy, Szaz); 
xCOJ=yy3;  ttOJ-tt; | 
ymax=yy3;  ymin-7yy;  xmax-2npt; xmin=0.03 
for (1=13i<npt3;it++) 

f 


#scanf (fpxyz,"\n Ad Af Af ZF", 
521 و 8۱۷/۷ و > 304 و‎ 82 2 ( 5 
xLi ]zyys tCij=tt; 


if (yy>ymax) ymax=yy3 
else if(yy<ymin) ymin=yy3 


pl س۷ر ا مج‎ 
> 
plot (ymax ,xmax ,xmin,ymin,x,t,choice) ; 
biosset (3,30); 
printf(" “~s ","Y EST vs TIME ST 
biosset (4,8): 
printf 4456 „6b (۳(5 
fclose(fpxyz); 
break: 
Í 
case 3: 
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fpxyzz2fopen("xkk.dat","r"); 
fscanf (fpxyz,"Nn Zd Zf? ZF Af'",&tt,E&Rxx,&yy, &zz)3 
xCOJ=zz3  tLOlJstt; 
ymax-zz$ ۷1 522 2 ؟‎  xmax-2npt; xmin=Q. Q0; 
for (i2z1zi<nptsi++) 
< 
fscanf(fpxyz,"Nn Zd Z7? Af Af", 
&tt,%xx “yy, ZZ); 
xLil2-zz;  tLilstt; 


if (zz >ymax) ۰۷/۱۵ : 
else if(zz&ymin) ymin-zz; 


ppsmtto 4o ee) 

> 
plotí(ymax,xmax,xmin,ymin,x,t,choice); 
biosset (3,30); 
printf(" 4s ","7 EGT vs TIME SLOT“), 
biosset(4,8); 
PAIDET AS O (FT) "s 
fclose(fpxyz); 
break; 
>; 

case 4: 

£ 8 š 
fpxyz-fopen("xkk.dat","rc"); 
fscanf(fpxyz,"Nn Ad Af Zf Af'",&tt, Xxx, &yy, zz); 
tLOlJ2xx; xLOlyy; 
yiax-yys;  ymin-yy;  xmax-7xx;  xmin-2Xxx;j 
for (12131<nptši++) 


fscanf (fpxyz,"\n Xd Xf Af ZF", 
81 و ( 2 52 و 5۱/۷ و 542424 و‎ 
tLil2xx; x[i]=yy; 


if (yy>ymax) ymax=yy3 
else if(yy&ymin) vminzyys 


if (xx >xXmax) xmax=xx; 
else if (xx<Kmin) xmin=xx3$ 


procirntfo As". 2 ٩ 
+ 
plot (ymax ,xmax,xmin,ymin,x,t,choice)s 
biosset (3,30): 
printf(" Zs ","Y EST vs X_EST"); 
biosset (4,8): 
DEINE SAS" SAFT) =) 
biosset (22,63); 
Stern f Geos seme ۳ 
fclose(fpxyz); 
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break; 
> 
case 9: 

t 
fpe=fopen("xdiff.dat","r"); 
fscanf(fpe,"\n Xd Zf “ZF 6 " و 5۱۷۷ و 52 و 911 و‎ 52 2 ( 4 
xLOlJ2xx;  tLOolstt; 
ymax=xx3  ymin-2xx;  xmax-2npt; xminzO.0; 
for (i=lsi<npts31i++) 

3 

fscanf(fpe,"\n Zd Zf 4+ ZF", 

و ( 2 82 و 8۱/۷ و 80202 و 81-1 
و 1 <<[ ۲1 2۶ [ 1 ] 2 


if (xx>ymax) ymax=xx$ 
else if (xx<ymin) ymin=xx; 


printt( As و و‎ S 

2 
plot (ymax ,xmax ,xmin,ymin,x,t,choice) ; 
biosset (3, 30): 
printf(" Xs ","X ERR vs TIME SLOT"): 
biosset(4,8); 
prirntf(o As 4501) oes 
fclose(fpe); 
Dreak; 
: 

case ôs: 

i 
fpe-fopen("xdiff.dat","r"); 
fscanf (fpe, "\n Zd AF Af ۸ ۲ و ( 2 22 و 5۷۱۷ و 500624 و 811 و‎ 
x[O]=yy;s  ttolstt; 
ymax=yy;s ymin=yy;s  xmax-2npt; 01 ۲520۵۰ 0 
for 1 21 5: 1 ۰۳۱۳۵ وا‎ +( 

{ 

fscanf (fpe, "\n Zd Zf “ZF Xf", 

؟ ( 2 82 و 5۷۷ و 524 و ۵41 
xLilzyy; trtil-2tt;‏ 


if (yyoymax) ymax=yy3 
else iflyy<ymin) ymin=yy;3 


printto As و‎ i 6ار‎ 

> 
plot (ymax ,xmax ,xmin,ymin,x,t,choice): 
biosset (3,30); 
printf(" 4s ","Y ERR vs TIME SLOT"); 
biosset(4,8); 
prímu + 46. (FP) Ji: 
fclose(fpe); 
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break; 
> 
case 7: 

3 
fpe-fopen("xdiff.dat","r"); 
fscanf (fpe, "kn Ad “ZF Zf Af'",Xtt,*xx, yy, zz); 
xLoJezz;  tLOlstt; 
ymax-^zz$ ymin=zz; xmax=npts xmin=0.0;5 
for (i=1;i<npt; i++) 

i 

#scanf(fpe,"\n 4d Af “ZF ZF", 

و 1 2 22 و 20/۷ و 304 و 841-1 
و 121 1 7 22[ x[i‏ 


1 ۶ (zz>ymax) ymax-zz; 
else if(zz<ymin) ymin=zz; 


٤ے‏ کے ےم ۶)۷ نامہم 
> 
plot(ymax,xmax ,xmin,ymin,x,t,choice);‏ 
biosset (3,30);‏ 
printf(" 4s ","Z ERR vs TIME SLOT");‏ 
biosset (4,9);‏ 
printf("A4s"," (FT) ")5;‏ 
fclose(fpe);‏ 
break;‏ 
> 
case 8:‏ 
< 
fpe-fopen("xdiff.dat",'r");‏ 
fscanf (fpa, "\n Xd “ZF ZF Wf',&tt,&xx, yy, zz);‏ 
و 1 121 ۲۱۵ xLOl2xx;‏ 
var COIJ=0.03; sum=0.05; sumsg=0.0;‏ 
ymaxk=xx 3  ymin-xx;  xmax-2npt; xmin-20.0;‏ 
for (1=131<npt31i++)‏ 


fscanf(fpe,"\n Xd Af “ZF ZF", 
Qtt, XxXxX و ( 522 و 5/۷ و‎ 
xLild-xx$ CELI ISTE; 


sum+zxLi ]: sumsq-*-xLil*xLili; 
varLil2-((tLil*1.0)*sumsq-sum*sum)/((tLil*1.00*t[11); 
xx-varLil; 


if (xx»ymax) ymax=xx 3 
else if (xx<ymin) ymin=xxs 


nrelmed (40 0). 

I 
plot(ymax,xmax,xmin,ymin,var,t,choice); 
biosset(3,30); 
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printf“ Zs ","X VAR VN TIME SLOTS 
biosset (4,7); 
printf? ) ۳/۰5۲ و‎ " )۳۴ ۱5 ۸ 4 
+ 21 وه‎ )0 6( : 
break; 
> : 
case 9: 
€ 
fpe-zfopen("xdiff.dat","r"); 
fscanf(fpe,"\n Ad Af AF 21" čEE XX VY 422): 
xLOl2yy; tr[O]=tt; 
vartOo120.0; sum=0. 03 sumsq=0.03 
ymax-2yys  ymin-7yy; xmax=npt; xmin=0.03 
for (i=lsi<nptsit++) 
£ 
fscanf(fpe,"Nn Zd Zf “ZF ZF", 
&tt,&xx,&kyy,&zz); 
xLil-2yy; tril=tt; 


sum+=x[ il; sumsq-*-xLil*xLil; 
varLtils((tLtil*1.0)*sumsq-sum*sum?)/((tLi1215*1.0)0 *tL i11); 
yy-7varLil; 


if (yy>ymax) ymax=yy3 
else if(yy<ymin) ymin=yy3 


printer 4S ۰٣٦٣ 

Jj 
plot(ymax,xmax,xmin,ymin,var,t,choice); 
biosset (3,30); 
printf(" 4s ","Y VAR vs TIME SLOT"); 
biosset (4,7)3 
۴۵۳ 1 ۲۱۳ ۶ ۱ 2۶5 ۰ و‎ ۰۳ ۱ ۵ 
fclose(fpe); 
break; 
> 

case 10: 

3 
fpe-fopen('xdiff.dat",'r"); 
fscanf(fpe,"Nn Ad Af ZF ZXf#",%tt,%xx,%yy, 2742 2 ( 4 
xLolJj2sezz; ۲۲0121 
vartO120.0;  sum-O.0; sumsq=0.0;3 
ymax-zzs$ ۷1 ۲۱<2 2 «max-npti xmin-20.0; 
tor (i=1;i<npt;i++) 

c i 

fscanf(fpe,"Nn Xd Zf at AF", 

5 ( 2 52 و 5۷/۷ و 4( 224 و 8411 
xLil-zz;  ttil-tt;‏ 


sum+=x [i J; sumsq-*-xLil*xLil; 
varL1]2((t01]+1.0) #sumsg—sum#sum) / ((t01]1+1.0)xt[1]1): 
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zz-vartil; 


if (zz»5ymax) ۷/۲۵ 22 2 3 
else if (zz<ymin) ymin=z2z; 


Printet (OAS ee 
> 
plot (ymax ,xmax ,xmin,ymin,var,t,choice) ; 
biosset (3,30); 
printf(" 4s ","Z VAR vs TIME_SLOT"); 
biosset (4,7): 
printf ("%s","(FTsq)")s 
fcloseí(fpe); 
break; 
> 


case 11: 


3 


#pp=fopen("pkk.dat","r"); 
fscanf (fpp,"\n Zd Af Af ZF tt xXx و 5۱۷۷ و‎ 82 2 ۸ 3 
xLOl2sxx; ۲0121  : 
ymax=xx3 ymin=xx3 xmax=npts xmin=0.0; 
for (i=1;1<npt3;it++) 

i 

fscanf(fpp,"Nn Ad Af Af ZF", 

و ( 2 82 و 2۷/۷ و 4 4( و 811 
xLil-2xx; tril-tt;‏ 


if (xx>ymax) ymax=xx3$ 
else if (xx<ymin) ymin=xx3 


te fe Cs s oe‏ و اه 
۱ > 
plot (ymax ,xmax ,xmin,ymin,x,t,choice);‏ 
biosset (3,30):‏ 
printf(" 4s ","PKK X vs TIME SLOT"):‏ 
biosset(4,7);‏ 
printf("Zs"," (FTsq)");‏ 
fclose(fpp);‏ 
break;‏ 
> 
case 12:‏ 


c 


fpp=fopen("pkk.dat","r")3; ۶إ‎ 
fscanf(fpp,"\n Zd “ZF Af 0۴ ۲ و 2۷ و 502424 و 2 و‎ 82 22 ( 4 
۲ ] 0 [2۷۷۶ ۲ ۶ 
ymax=yy3; ymin=yy3; xmax=npt; xmin=0.0; 
for (12131<npt31++) 
1 
fscanf(fpp,"Nn Zd Zf Zf Zf", 
#¿tt,%xx,%yy, 2 2 ( 5 
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xLilzyy;  tLilstt; 


if (yy>ymax) ymax=yy3 
else if(yy&ymin) yminzyy; 


printf 5st. ل‎ 

+ 
Plot (ymax ,xmax ,xmin,ymin,x,t,choice) ; 
biosset (3,30): 
printf(" 7۶5 ","PKK Y vs TIME SLOT"); 
biosset(4,7); 
printf ("4s" , “(eisai 5 
fclose(fpp); 
break; 
? 

case 13: 

t 
fpp=fopen("pkk.dat","r")s 
fscanf(fpp,"\n Ad Af Af Af'",&tt,&xx, &yy,&zz); 
x[OJ=zz;  t[OJstt; 
۷۲3 22 2 ؟ 2 ۲۱522 ۷/01 ؟‎  xmax-npt; xminzO.0; 
for 11 ۶ 1 ۰۳۱۵  ( 55 

fscanf(fpp, Np Au WELT AE و‎ 

: ( 2 852 و 5۷۱۷ و 36 ايع و 211 
5 تال ۳1 .۰ 2 122 ۲2 7۸ 


if (zz>ymax) ymax=zz; 
else if(zz<ymin) ymin=zz; 


Drintft' "As 7145): 
? 
plot (ymax ,xmax ,xmin,ymin,x,t,choice) و‎ 
biosset (3,30): 
printf(" Zs ","PKK_Z vs TIME _SLDT"); 
biosset (4,7); 
printf ("As و‎ (Pa oe es 
fclose(fpp): 
break: 
? 
case 14: 
x 
fpt-fopen("trj.dat"," nr» 
fscanf(fpt,"Nn Zf Zf ZF" &xx, &yy,&Z2) § 
xLOJzxx; ylOl=yy; 
ymax=yys ymin=yys:  xmax-2xx;  xmin-2xx; 
tor ۰ 1 51: ۰۱۳ ۰ ۶ 
t 
fscantf(fpt," Nn Wf Aff tcu IET 
xLil2xx; ylid=yy; 
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if (yy>ymax) ymax=yy; 
else if(yy<ymin) ymin=yy3; 


if (xx>xmax) xmax=xx; 
else if (xx<xmin) xmin=xx3 


prrmtfo As ۳ ٤ 
> 
plot (ymax ,xmax ,xmin,ymin,y,x و (66 6۳۱۵1 و‎ 
blosset (3, 30): 
printf(" As ","TRUE Y vs TRUE X"); 
biosset (4,8); 
BENET OVS" و‎ ۲ ۳۳۱ "( 
biosset (22,63); 
Drintft("Zs" ,"(FT)")5s 
fclose(fpt); 
break; 
> 
case 15: 
i 
printf ("An CHANGE PLOT ( enter option )"): 
6۵۳1 ۲۳ ) ۲۱۳۳۲۱۸ : | 
‘printf ("\n 1) LINEAR INTERPOLATION BETWEEN"); 
printf (" POINTS"); 
printf ("\n 2) SIMPLE DOT"); 
(۳ 1۳۱۸۵۲ ۲ ۱۳۱۳ z 
printf ("\n ENTER choice : "); 
scanf ("Zd",&choice) ; 
goto skip; 
break; 
> 
case 16: goto ending; 
default: 
€ 
goto skip; 
break; 
> 
> /* end case */ 


CUR MV (4,47): 

printf ("Zs 4d", "# ",runn); 
CUR. MV (35,67); 

if (narray==0) printf ("SINGLE"); 
else printf ("MULTIPLE"); 


c-keyrdtí)1 
Diosini (3); 


skip: 


; /* end for-forever-loop */ 
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ending: printf("\n");5 
printf#("\nEnd plotting ... Press any key"); 
Czkeyrd(): 
CLR SCRN; 


/ 3 Jede 3C HE He 3E 3E CE 3E 3E 3E 3E JE 3E 3E 3E 9E 3E 3E 3E 9E 9E 9E E HHH E E 9E E 9E JC 9E E 9€ 9C JE EE 9E 9E 90€ 9E 9€ 9€ E XE 9€ 9€ / 


/% */ 
/* Function to draw two axis. */ 
/% */ 
/* **4 NOTE s* */ 
/* The upper left corner of the screen is 0,0 and the */ 
/* bottom right corner is 199,639. */ 
/% */ 


/ 3C JC 3C 3E 3C 3E 3C 3C 9C 9€ 3E 3C 9E 9E 9E 3E 3E E JE 3E 3E E JE 9E 3E JE JE 9E 9E JE JE E 9E JE JE E HE 9E 9E 9E 9€ 9€ 3E 9E 9E 9E 9E 3E 9E 9E 9E 9393€ 9€ 9€ 9€ / 


int axis(ulrow,ulcol,lrrow,lrcol,mantx,manty) 

int ulrow,ulcol,lrrow,lrcol; 

double mantx[2J,mantyl[1; 

{ 

int count,irow,i,iaux,icol,tcount,cinc,rinc,hinc; 

double 

deltax ,deltay, ymax ,xmin,x ,y,rowmax ,rowmin,colmax ,colmin; 
double deltar,deltac,aux; 


ymaxzmantyL0J: 

xmin2mantx[11; 

deltayzOo.1*xí(mantyL[OJ-mantyL[123); 
deltax2z0. 1# (mantx[O]-mantxL11): 

rowmaxzlrrow:  rowmin-ulrow;  deltarzO.i*í(rowmax-rowmin); 
colmaxzlrcol: colminzulcol; deltac=0.1*(colmax-colmin) ; 


rinczdeltar+0.53: Cinc=deltac+0. 5; 

/* اس سن سن ع سن سن ل م سن ااا et‏ —————————— */ 
#x do Vertical ------ line with scale */‏ 

i-ulcol;j 


for (irow = ulrowtls irow <=-Ilrrows; irow++) bioswd(irow,1)3 
for (irow.- ulrow; irow $7 lrrow; irowrt-2rinc) 

Tor (tcountzi-4: tcount <= 17 tcount++) 
bioswd(irow,tcount): 


1“ do horizental سس سس‎ line with scale */ 
1=lrrow; 
for (icol -» ulcol-*1; icol <= lrcol: icol++) bioswd(i,icol); 
for (icol - ulcol; icol 4$» lrcol; icol-*-cinc) 
for (tcount=1;5 tcount <= 1+4: tcount++) 


bioswd(tcount,icol); 


/* write vertical scale */ 

Yy-yfax; 

for (irow = ulrows irow £7 lrrow; irow-t-2rinc) 
3 
i-irow/8.0-0.5; 
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biosset (1,0); 
printf (C A:lft y); 
y-=deltay; 

> 


/* write horizontal scale */ 
x-xmin; 
tor (icol = ulcol; icol <= ۳۱۵1 8 1 ۰91 2۳۱ ۱ 
X 
i =i CO1#0. 125-1. 5; 
biosset (24,1): 
Pr Pn PO Tp X, 
x+=del tax; 


/* done */ 
return (O); 
> 


plot(yymax,xxmax,xxmin,yymin,x,t,choice) 
float yymax,xxmax,xCJ,tli,xxmin,yymins 
int choice; | 


float xscale,yscale,height,width,bx,by; 

float ymax,xmax,ymin,xmin; 

double xaux,yaux,mantxL21,mantyL22,109g10(); 

double aux,pow(),aux1; 

int i,inc,row,col,ixexp,iyexp,c,oldrow,newrow,ii,iimax; 
int coli,colO,rowi,rowO,irow,oldcol,icol,newcol; 

float deltac,deltar,delta,ang; 


/* DETERMINE THE SCALING */ 
ymax-yymaxs xmax-xxmaxs; ymin-2yymins xmin-xxmin; 
if (ymax==ymin) 
c 
ymax —ymax +5. 0s 
ymi nzymi n-5. 0s 
7 
1f (xmax==xmin) 


< 


xmax-xmax-t*5.0; 

xmin-2xmin-2.0; 

> 
height=ymax-ymin; 
widthzxmax-xmin; 
xscale-560.0/width; 
yscale--160.0/height; 
by=184. O-ymin*yscaleto.5; 
DX 5600. O-xmax #xscale+0. 3: 
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/* DETERMINE EXPONENT and MANTISSA */ 
xaux=1.03 
aux-absí(xmax); auxi1-2abs(xmin); 
. if (aux < aux1) auxzaux1: 
if (aux !=0.0) 
i 
xaux-logiO(aux); 
ixexp-xaux; 
if (ixexp<O) ixexp-=1; 
aux-ixexp; 
xaux-pow(10.0,aux);j 
>; 
mantx COJ=xmax/xauxs$ 
mantxL1]-2xmin/xaux; 


yaux-21.0; 
aux=abs (ymax); auxl=abs(ymin) ; 
if (aux € aux1) aux=auxl; 
if (aux !=0.0) 
{ 
yaux-logiO(aux); 
iyexp^yaux; 
if (iyexp<O) iyexp-=1; 
aux=1yexps; 
yaux-pow(10.0,aux); 
> 
mantyLOl-ymax/yaux; 
mantyL1]-2ymin/yauxi; 


printf("XnAs","Press any key to see plot."): 
czkeyrd():s 

biosini (4); 

axis (24,40,184,600,mantx,manty) 5 


yaux=yaux$ 
biosset (3,6): 
printf ("Z.le",yaux) 5 


KAUKHK AUX 5 


biosset (22,68) ; 
printf ("Z.le" ,xaux) و‎ 
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/ kkt bt ید‎ 9E 9E 9€ 9€ 9€ 96 9696 9E 9E 9€ 9€ 9€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ JE 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9E 9E 9€ 9€ 9E 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ HHH HHH / 


/* * / 
/* PLOT USING LINEAR INTERPOLATION BETWEEN POINTS ۸ 
/% + 


/ X 3€ 3€ 3E 3E 9E 3E 9€ 3E 3€ 3€ 9E 9€ 9E E 9€ 9€ 9E KE KE KE 9E KE KE 9€ 9€ 9€ 9€ 9E 9€ 9C 9C 909€ 969€ 9€ 96 96 96 9€ 9€ 9€ 96 90 9€ 9€ 9€ 9€ 9€ 9€ 969€ 9€ 9€ 9€ 9€ 9€ / 


if (choice==1) 
€ 
colo-zttOl*xscale-cbx; 
rowO=x COl*yscaletby; 
bioswd(rowOo,colO); 


for (1=131<npt31++) 
t 
coliztíil*xscale-bx; 
rowizxLil*yscale-by; 
deltaczcoli-colO; 
deltarzrow1-rowôs 


if (abs(deltac) >=abs(deltar)) 
t 
ang=deltar/deltac; 
iimaxzabs(coli1-colO); 
1col=1; 
col=colQ; 
if (coli<colO) 


3 
1col--1: 
+ 
for (11=0O311<1imax31iit+) 
€ 


col+=icol; 
deltazcol-colo: 
row=ang*deltat+row0+0.5; 


/* if (choice==1) 


+ 


1 ۶ (abs(deltar) > 5 && abs(deltac) >5) 
goto discont; 
J E #7 


bioswd (row,col); 
> 
> /* end_if abs (deltac)>=abs(deltar) */ 
else if (abs (deltac)<abs(deltar)) 


{ 
ang=deltac/deltar; 
limax-abs(rowi-rowO); 


2 ۲۳ ۵۷۷< 1 
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row=rowd; 
if (rowi«&rowO) 
{ 
irow=-1; 
+ 
for (iizÔsiišiimaxsii ++) 
1 
۳ و بیان ۳ 1 <+ ان‎ 
deltazrow-rowôs 
colzang#delta+co10+0.5: 
/* if (choice==1) - 
t 
if (abs(deltar) > 5 %% abs(deltac) >5) 
goto discont; 
+ */ 
bioswd(row,col); 
> 
> /* end else if */ 
201 026601 1 :  rowo-rowi; 
; /* end if for i=0,200 ۸ 
+> /% end if choicez1 */ 


9E 9E 9E 9€ /‏ جد جد جد جد جد جد 9E 9E 9E 9E 3E‏ 9€ 9€ جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد جد HHH HH HH HH HHH HHH HH HH‏ | 


/* : * / 
/* This portion is to be used if you want just the %/ 
/* points without linear interpolation. */ 
/* */ 


/ 3t 3E 3E 3C 3C 3€ 3€ 3€ 3C 3€ 3E 9E 3€ 9€ 3E 9E 96 3E C 3C 9C 3€ 9E 3C 3 9E 96 9E 3C 9E 9e 9€ 3C 3C 9C 96 3€ 3C 3 9E 9E 963€ 3€ 9E 9E 96 3€ 9€ 9E 96 9€ 9€ 9€ 9€ 903€ 9€ / 


if (choice==2) 


< 

for (i=O;i<npti;i++) 
< 
col=t[i ]l#xscale+bx: 
row-xLil*yscale-tby; 
bioswd (row,col); 
> 

+ 

goto termino; 

discont: 
i 


biosini (3); 
printf("Nn DISCONTINUOUS DATA FOUND."): 
printf(" CHANGE OPTION to SIMPLE"); 
printf("-DOT PLOT and TRY AGAIN."); 
printf(" Press any key to continue ."); 
c=keyrd();5 
H 

termino: 

> 


APPENDIX H 


LISTING OF THE BATCH FILES TE FEEL 


/ 3€ 3€ 9€ 3€ 3€ 3€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 9 9E 9€ 9€ 3€ 9€ 3€ 9E 9€ 9€ 9€ 3€ 3€ 9 3€ 3€ JE 9€ د جد‎ 9€ 9€ 9€ 9€ JE 3€ 9E 9€ 3€ 9€ 9€ 9€ 96 9€ 9€ 3€ 3€ 9€ 39€ 96 9€ 9€ 96 39€ / 


/% */ 
/% Batch file "CE. BAT" (IBM-PC). Contains the */ 
ix sequence of calls to compile thefile KR.C . */ 
/* * / 
ix USAGE: "ce kr" */ 
/% */ 
eee ECCE He o RR Ae GEEK OK Oe Je de HE e Ae dece de db dide CAI EHE e de e KE KE ب بهذ به بذ بق‎ 
lci Al 

1 2 1 


link cs+41+biosiottrt+dostime,41,nul,lemstlcs 
erase ۸1. ob j 
pause 


/ e e He He Je De 3e 3E DE DE DE DIE DIE JE IE 9E 3E JEJE JC IE JE IE JEDE 3C 3E JEJE JE 9E E JEDE 3E 3E JE JEJE DE JE 3E 3E 3E 3E 3E JE E HE 9E 9E EXE XE 3E EAE / 


/* * / 
/+ Batch file "KAL.BAT" (IBM-PC). Contains the */ 
/% sequence of calls to execute the files KR.C * / 
/% (Kalman Filter program) and PLOT.C (the * / 
2 Plotting routine). * / 
/* USAGE: "kal" */ 
/* */ 
aT EHE HE EE CAE EE ICE EO IEEE EIE E E KF KE E EF FF FF FF FF FF FF جر دج جه‎ 7 
graphics 

: 1 0 

kr =4000 

rem DO YOU WANT TO PLOT RESULTS 2 

pause 

plot =8000 


rem DO YOU WANT TO TRY ANOTHER CASE ? 

rem CTL-BREAK will stop execution. Press any other key to 
continue. 

pause 

ELS 

goto loop 
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APPENDIX 7 


SIMTERM - PROGRAM DOCUMENTATION 


This Appendix contains the listing of the file "readme" 
found in the PC-SIG LIBRARY (Public Domain Library), disk 


number 362. 


hJ 
b 
N 
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dou Introduction 


Feel free to make copies of this program. There is 
no restriction on the distribution as long as the copyright 
notices are left on the source: and that there is no charge 
for the distribution. This program is made available to 
users of IBM-PCs (and compatibles since I really like the 
COMPAG/PLUS). No guarantees are made or implied. If you have 
any questions or suggestions, please give me a call and I 
will try to answer them. A recent acquisition of an 
interrupt driven communication driver from the University of 
British Columbia Computer Center has enabled me to remove 
the requirement that you have the IBM Async Communications 
Support package which included a driver I used to use. This 
version of SIMTERM is therefore self-contained and requires 
no other software to run. Have fun using it and **HAPPY 
COMPUT ING**. 


This floppy contains all the software for the 
terminal simulator for the IBM PC. The main function of 
this program is to provide a reasonable terminal simulation 
on the IBM PC of an HP-like terminal to a UNIX(TM) system. 
The file "simterm.exe is the load module and when invoked, 
assumes a default of 1200 baud, 1 stop bit, no parity, and 
flow control (XON/XOFF). The only programs that have to be 
on the floppy are SIMTERM.EXE, HELP.TXT, and MENUS. If you 
have a VENTEL or a Hayes modem, the file BOARDS can be 
copied over as a starter for alist of BBSs. The BOARDS file 
is set up for a VENTEL autodialer. The dialing strings will 
have to be changed to conform for the Hayes standard. 


If you have DOS 2.0, put SIMTERM.EXE, HELP.TXT, 
BOARDS and MENUS in a subdirectory called \simterm. That 
Way, if you change directories during a SIMTERM session, by 
specifying a path for any type of file transfer, SIMTERM 
will be able to find its way back to these two important 
files. If you don't have DOS 2.0, then just IGNORE the 
Occasional "directory not found" messages, they re harmless. 


A Command Line Parameters 


Optional parameters on the command line will 
Override the defaults as follows: 
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-a“ put the simulator into the ADM3a emulation mode 
for those systems which don’t support TERMCAP. 


"-b^ Causes SIMTERM to assume you are using a 
monochrome display, even though you may not really 
have one. 


AS.) Will cause the transmit and receive buffers to be 
cleared when a BREAK (End key) is sent. This is 
mainly for high baud rate lines where there is 
still junk in the buffers that you don’t want to 
see. 


Dec specifies which  COMx device you want to use. The 
default is 1 (use COM1). 


"=G; Enter direct connect mode. SIMTERM won t wait for 
carrier detect. Use this option if vou re 
connected directly to the remote, with no modem. 
You can also use this option when SIMTERM seems to 
hang at startup, with the "Establish 
communications link" message. This may be case 
with Hayes modems if you don’t have them optioned 
to raise the carrier detect. 


"-D' When this flag is used, SIMTERM will ignore DEL 
(127 Ascii) characters, and the character 
immediately following the DEL. We use it because 
we have a noisy line. 


`_e specifies that the EPSON printer does not have the 
GRAFTRAX option. Therefore when using ‘ibmpr’ to 
route stuff to the printer, the code will simulate 
underlining, subscripts, and superscripts. Default 
is an EPSON with GRAFTRAX. 


pg Causes  SIMTERM to assume you are using a graphics 
display, even though you may not really have one. 


7 SIMTERM will operate in the 'half-duplex' mode; 
i.e., it will echo the characters the user types 
since the host system does not. 


crm specifies the IRQ (interrupt request level) to be 


Copyright @ 1982,1984 Jim % Eric Holtman 


SIMTERM Instructions Page: 3 


used. Default is 4, but some boards (like the 
Quadram) use IRQS for COM2, so in this case 
SIMTERM would have to be invoked with the 
parameters ‘-c2 -i$j' to set it up correctly. 


Em. Tells SIMTERM that you have a Hayes modem instead 
of the default VenTel modem. This will enable 
SIMTERM to use the directory dialing features 


(Alt-V) 

EN" Redefines the cursor control keys compatible with 
a set of UNIX-like utilities on a PRIME computer 
at UofVa. 

'-nXXXxXXx' 1f .you have a Ventel or Hayes modem, this option 
will automatically dial the number when the 


simulator is started. 


-O0 turns of f the error messages caused by 
communications line: ۰ ۰۰۰ framing error and 
invalid parity. 


—px specifies the parity where  'n'-none, 'o'-odd, 
'e'-even, 'm'-mark, and 's'-space. 


'-PFfil' Use file "file" as a script source file (see 
SCRIPTS below) 


'-PDscr' Execute script "ser" upon start-up (see SCRIPTS 
below) 

Er this causes SIMTERM to wait for the horizontal 
retrace when writing to the screen. If you notice 
` sparkles on your graphics monitor, invoke 


SIMTERM with this option and it should clear up. 


"-snnnn“ specifies 'nnnn' as the baud rate (e.g., < ., 
zone specifies the number of stop bits (1 or 2) 
'—vxxxx' specifies the file (xxxx)d to be used when the 


Alt-V options is invoked to dial BBS. The full 
path name of the file should be specified. 
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'—wn' set the word length to `n’. Default is 8B (7 data + 
1 parity). 

sm this option will turn off the XON/XQOFF flow 
control. The program will send an XOFF when the 
receive buffer is almost full and then an XON 


when it has emptied out. It will also recognize an 
XOFF from the host to stop transmitting and then 
an XON to start again. 


'-kfilename' will read in the file ‘filename’ to 
initialize the function keys. The file is of the 
form 


nzstring of chars 
where O<=n<=9 
example 
O-who^V 
1-whoi!wc^V 
If the string is terminated with a '^V' (chr(174) 
-- obtained by holding down the ‘Alt’ key and typing 174 on 
the key pad), a ‘Carriage return’ will be appended. 
For examples: 
If you want to run at 9600 Baud, even parity, 
without waiting for carrier detect 


simterm -s9400 -pe -d 


If you want to use COM2 (and it requires IRQ3) at 
300 baud and run script "xyzzy" from file "scr2" 


simterm -s300 -c2 -i3 -PFscr2 -PDxyzzy 


If you are using a Hayes modem and want a number 
dialed when SIMTERM is started, use the following command 
line: 


simterm -m -d -n12145551212 
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$3. Program Functions (Unix specifics) 


Once loaded, the program will use all available 
memory above the module for save space for paging through 
previous output. I have 256K on my system and this provides 
about 20 pages (“500 lines) of saved text. The TERMCAP entry 
for the terminal is in the file ‘termcap.ibm’. This should 
either be added to your termcap library or the ‘shell’ 
variable TERMCAP should be set to it before trying to use 
any package that requires cursor addressing. The TERMCAP 
entry makes use of the fact that ‘vi’ (and 'curses') will 
use a special sequence to enter and leave the control mode. 
The simulator uses these sequences to redefine the cursor 
control keys so that they can be used in that mode; e.g., in 
‘vi’ the "up arrow" will send in a "k", while the "PgDn" 
will send in a "^D" to scroll the display down, etc. For 
those of you who are running on UNIX systems that use 
"terminfo", there is a file "terminfo.ibm" which has the 
appropriate definitions. 


Output to the printer can be initiated under 
program control with a filter similar to ‘hp’ which is used 
for HP terminals. The source file for ‘ibmpr’ (which is the 
filter that use on UNIX) is on the floppy. You should 
compile this UNIX and then use for controlling i to the 
printer. ñ typical use of this filter is: 


nroff -T37 file i 1۳۳8۳ -p 


The ‘-p’ option says to route the output to the 
EPSON printer. The '-i' option will cause underlined text 
to come out in italics. The '-c' option is used to invoke 


the compressed mode (132 characters/line). 

If those escape sequences are output, they will be 
displayed appropriately on the CRT and output to the printer 
correctly. 

4. User Defined Function Keys 

There are 10 user definable functions keys that 


will send a character sequence of up to 30 characters. To 
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display the current contents of the keys, hold the "AIt“ key : 


* e 


down and type =’. To change the contents, hold the ‘Alt’ 
key and type -“ and follow directions. To invoke the 
functions keys, hold down the ‘Alt’ key and type the 
corresponding digit  (e.g., “Alt 3°). A user can define a 
file with the contents of the keys and cause it to be read 


in with the '-k' option on the command line. 
5. Upload/Download (Unix Specific) 


This is used to transfer ‘text’ files (ones that 
only contain printable ASCII and tabs) between the PC and 
the host. To remember which way the data flow, just remember 
that the IBM PC looks ‘up’ to UNIX, while UNIX looks ‘down’ 
on the PC. To move files between the PC and UNIX, hit 'Cntl- 
PgUp' to transfer PC-»5UNIX. Answer the prompts. Hit 'Ctrl- 
PgDn’ to move from UNIX- oPC. As the transfer is taking 
place, a message will indicate the # of bytes transferred. 
When going UNIX->PC, there will be more bytes on the PC disk 
than on UNIX because of added ‘end-of-line’ and ‘end-of- 
text’ bytes. This can only be used to transfer TEXT files. 


6. Upload/Download (Straight ASCII & XMODEM) 


The program can be used to connect to BBSs and 
any other systems that support async devices. To download a 
file to the PC, go through the normal dialog on the BBS and 
then when it is waiting for you to hit return to start the 
transfer, press Alt-C to open the ‘copy’ file that will 
receive the data. You will be put back to the original 
screen where you can hit return. At the end of the data 
transfer, hit Alt-C to close the copy file. 


To upload a file from the PC to a BBS, again go 
through the dialog to setup the transfer and when the BBS 
is ready, press AIt-D to specify the data file. This will 
then ‘dump’ the file to the comm line. At the end of the 
transfer, you may have to type in a control sequence to 


indicate that you are done. This option will request a 
‘delay’ between characters so that you do not overrun the 
receiving system. Use `O’ for no delay; a value of `i’ is 


probably sufficient for most systems that can not handle the 
normal baud rate. Experiment. 
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SIMTERM supports the  XMODEM protocol for data 
transfer from a BB. To invoke this option, hit Alt-R to 
receive with XMODEM at the point that the sender is ready. 
You will be prompted for the file to receive into and then 
the PC will initiate the transfer of data. Transmitting with 
XMODEM is invoked with Alt-T and prompts will follow. 


7. Script files 


Script files are used to allow SIMTERM to do 
mindless interactive login and other such sequences. A 
Scriptfile can be created by your favorite word processor 
and SIMTERM will compile it for you, and save the compiled 
version so you don't have to re-compile at each run. Script 
files read much like English, so they are both easy to 
understand and create. 


Script Commands Available 


Each of the following commands can appear in a 
script file. Note that each separate command must be 
separated by a space, tab or newline. The compiler ignores 
extra white-space, so you can have indenting, multiple 
commands per line, or whatever you want. Syntax notes: "str" 
in the following denotes a string constant delimited by 
quotes. A string can have multiple words. To get a quote 
character into a constant, write it as N". To get a 
backspace, write it as \\. Although one-word strings need 
not have quotes, they should, so that they are not confused 
with keywords (commands). Non-quoted strings generate 
warnings from the compiler. 


name "str" --- defines an entry point for the 
script that you can use to identify this part, i.e, if a 
“name whuxlb" appears in the file, then you can start 
running the script by using the name "whuxlb". (See "running 
scripts" below) If you choose “whuxlb" from the pop-up menu 
later, the script will start executing at the statement 
immediately following the "name" statement. 


send "str" —— sends the string to the remote 
computer, with a carriage return tacked onto the end. Some 


escapes can be placed into the string, as follows: an "\1" 
causes a one second pause when it is encountered. "Xm" sends 
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a carriage return. "Xj" sends a line-feed. "\c" Suppresses 
the carriage return that is normally tacked onto the end of 
a string. Remember, though, you need to type TWO backspaces 
to get one, so these sequences should appear as \\1,\\m, etc 
in your file. | 


dial "str" سس‎ dials the string on a VenTel 
dialer. Obviously, if you don’t have a ventel, don’t use 
this command (Fake it with "send"). For example, to dial 
"3395" on a Hayes, put a line like "send ATDT3395" in your 
file. 


say "str" سس‎ outputs "str" onto the screen, so 
that you can put tracer statements in the script. 


goto "str" --- branches to str. labels to be 
branched to must be followed by a colon. I.e. early in the 
script you have al line "labels" all by itself. Later, you 
can call "goto label" (no colon) to Branch back up to the 
earlier statement 


gosub "str" سس‎ like goto, but when a "return" 
statement is encountered later, the program branches BACK to 
the statement that follows the gosub. Gosubs may be nested. 


return --- returns from a "gosub" statement 


input "str" سس‎ Prompts the user with "str", 
reads one line, and sends it to the remote computer. 


settime "str" --- sets the timeout value to "str" 
seconds. Default is fifteen. 


quit ——— execution is returned to SIMTERM. 
There is an understood "quit" at the end of file 


The Logical Operators (IF/CASE) 
Besides the COMMANDS, the Script compiler 
recognizes two logical operators, the if/else statement and 


the case. 


IF SYNTAX 
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if "str" stmnti1 else stmnt OR JR erp i 
stmnt 
stmnt 
OR Š 

if "str" stmnti1 (no ELSE) + else stmnt 
OR ? else « 
stmnt 

>? 


When encountered, the program will scan the output 
from the remote computer until either it finds "str" or it 
times out. If it finds "str", stmntl is executed, if it 
times out, stmnt2 is executed. Execution then drops down to 
the next statement, unless the stmnti or stmntZ contained a 
goto or gosub or quit. 


stmnntí or stmnt2 may be either single commands 
from above, or multiple commands delimited by ¢ and + (note 
white space around the brackets. If the else following the 
if is missing, then execution falls to the next statement. 
Note that ifs may be nested. For example 


if "str" dial "4244" else if “str2" dial "4324" 


Be aware, however, that input is only scanned 
once, so if “str2“ appears Before "str" in the preceeding 
example, both tests will fail!! 


CASE SYNTAX 


The case statement is like multiple simultaneous IFs. 
It allows you to set up a few strings to be searched for 
SIMULTANEOUSLY, with an action specified to be executed if 
and when one of the strings is found. Syntax is 


case 
stri: action 

5 ۳2۶: action 
otherwise action 
caseend 
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Action can be -either a single statement (say, 
send, dial, or even an iif, or another case.) or multiple 
Statements surrounded by t and ? (like the if clause.). For 
example, if you have 


case 

eric: dial "eric’s #" 
Holtman: dial “another #" 
caseend 


and the input is "djsfhj gfijg eric holtman" then "eric's i" 
will be dialed and the case will quit. Note that "holtman" 
is not matched since a case statement executes as soon as 
any of the strings is found. 


Script Examples 


Call number 3395, wait for "login" prompt. If I 
get it, gosub to a login procedure. If I don't then say 
something, then quit. The login subroutine will send my id, 
then prompt for my password, send it, then return. 


name example 
dial 3395 i 
if "login" gosub "subr" else < 
say "3395 didn’t answer" 
quit 
7 
quit 
subr: 
send "1 3454" 
input "Your password, sire? " 
return 
Note that after the "return" the next statement to 
be executed will be "quit". 


Compiling Scripts 


To use the example above, copy it out toa file 
called, for example, "prog". Then invoke SIMTERM with a - 
PFprog flag, like so: "simterm -PFprog". This tells SIMTERM 
that you wish to use scripts from the file "prog". If no -PF 
flag appears, SIMTERM will read the file "scripts" which is 
where you may want to keep compiled scripts. After SIMTERM 
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gets running, type an Alt-L. This informs SIMTERM that you 
wish to use a script. It will examine the file "prog" and 
determine that it needs to be compiled. Tell SIMTERM what 
file you want to put the compiled stuff in, then sit back 
and watch for error messages. After it compiles, hit return 
and proceed to the next step. Remember the file name you 
used for the compiled scripts, and use it next time you run 
SIMTERM, so that you don't have to recompile everything 
again. 


Using a script 


After you type Alt-L, or after the compile is 
finished, a menu of all "name" strings will pop up. If 
you're still using the example, only one name ("example") 
will be there. Using the up and down arrows, make your 
choice, then hit the space bar. SIMTERM will then execute 
your script starting at that named point. 


To execute a script from startup, use the -PD 
flag. For example, to run script "example" from the file 
"prog.scr", type 


simterm -PFprog.scr -PDexample 
= Functions Available from the Keyboard 


All these functions can be accessed from the 
keyboard by typing the appropriate character. If you forget 
the commands, Alt-H will Bring up a menu for you. Just 
follow the menu selection process until you find the desired 
function. Note that the menu will tell you which key to use 
to activate that function in the future. I.e., the menu has 
Alt-L next to the script choice to remind you that you can 
use Alt-L instead of the menus to activate the script 
functions. 


E Will 'toggle' the display so that usually 
non-displayed control characters are displayed in reverse 
video using the upper case letter that they are equivalent 
toi 

E2 Will ‘print’ the line that the cursor is 


positioned at. 
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FS Send a BREAK 


F8 Freezes the display above the cursor. 
Handy to save some data on the upper portion so you can see 
it as you do other 
commands. 


F9 Turns on the row/column display at the 
bottom of the screen so that you know where the cursor is 
currently positioned. 


F10 Toggles XON/XOFF. A convenient way of 
stopping/restarting the output to the CRT when connected to 
a system that honors the XON/XOFF protocol for flow control. 


۸11-۳ 1 Will terminate SIMTERM, but leave the DTR 
line high so OR that you can restart SIMTERM and pickup 
where you left Alt-255 off. 


8۵10-2 Terminate SIMTERM, and drop DTR so that 
the modem will hang up. 


alt-F3 Toggles the DTR (data terminal ready) 
lead on the communications line. This is used to drop the 
carrier at the modem so that you can redial. 


Alt-F 4 Toggles the graphics-mode. This will 
display characters with their parity bits so that the 
“graphics’ characters of the PC (5127) are should. This is 
used on some of the BBSs. 


۸1 ۲-۳5 Clears line 25 if it becomes cluttered. 
Alt-Fé Display various mode settings on line 25. 


Alt-F7 Resets the communications software. In 
some instances an interrupt might be missed and leave the 
system in a state where it is expecting data that will not 
be sent. If you suspect this, try an Alt-F7 before rebooting 
the software to see if it breaks you out of the situation. 


Alt-C Puts SIMTERM in a ‘capture’ mode so that 


all input received over the communication line is logged to 
a disk file. Handy for picking up ASCII text from a 


Copyright @ 1982,1984 Jim ® Eric Holtman 


SIMTERM Instructions Page: 13 


system that does not support XMODEM. It will prompt you for 
a file name. 


۵1۱1 ن0ا‎ Will ‘dump’ a ASCII file on the FC to the 
comm line. This is a way of ‘uploading’ to a system that 
does not support  XMODEM. You can also specify a delay to 
prevent overruns. 


Alt-E Toggles the ‘vi’ mode. Handy when you 
exit SIMTERM and then came back into it with UNIX still in 
‘vi’. The mode should be TRUE to have the cursor keys work 


۹ . 


for VI “5s 


Alt-F Enter the "local line editing mode. This 
allows you to make corrections to a line and then reenter 
it. Use the cursor control keys to move to the line, ‘Ins’ 
and ‘Del’ to fixup the line and then hit return to reenter 
the line and leave the line edit mode. 


۵1 ۲ Display a ‘help’ menu listing the defined 


function keys. 
alt-L Invoke the 'script' processor 
81-8 Receive a file using the XMODEM protocol. 
alt-T Send a file using the XMODEM protocol. 
۸1 1 با‎ Will display a menu that is used with the 
"auto-dial“ Capability of the Ventel. Once in this menu, 


select the desired feature. 


Alt-= Display the user defined function keys 
alt-- Change a user defined function key 

Home Goto top of file in "vi" (unix specific) 
Ins Send the current line to remote (similar 


to the ENTER key on an HP-Z621) QR if in "vi" toggle insert 
mode. 


Del Clear display from cursor to bottom OR if 
in "vi" delete a character 
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Arrows Move through screen memory in the 
specified direction 


PgUp Back up one page through memory 
PgDn Go down one page in memory 
Cntr+ Toggle output to printer 

Pr tSc 
Ctr1+ Download (Unix ---> IBM) 

PgDn 
Ctr1+ Upload (IBM ---> Unix? 

PgUp 
D Remote Control of SIMTERM 


Certain sequences of characters, when received 
over the communications line, will cause SIMTERM to execute 
certain functions, like clearing the screen, moving 
characters, turning the printer on and off. In the following 
descriptions ESC is Escape (ASCII 27), and Ctrl-X denotes 
the character generated by holding down Ctrl and typing X. 
I.e. Ctrl-A is ASCII 1. Note: in the descriptions "ESC H F" 
for example means ESCAPE, capital H, capital F, with NO 
SPACES. : 


ESC ñ —- move the cursor up one line 

ESE B —— move the cursor down one line 

ESC E ~~ move the cursor right one space 

ESL D سس‎ move the cursor left one space 

ESC È -~ take SIMTERM out of INSERT mode 

== = = —— remotely program a function key. After 


the F, send a number from 1 to 9 to indicate which key to 
program, Or a O to program key 10. Everything following the 
numeral, up to, but not including a terminating Ctrl-Z is 
then stored in that function key. (See User Function Keys, 
above) 


ESE H سس‎ moves cursor to top left of screen 
(HOME) 
ESC K ~~ clears the rest of the line, from 


cursor position over 
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ESE سب ل‎ clears display from cursor to end of 
line, and all lines below the cursor 

ت٣ سب‎ inserts a line above the cursor by 
rolling bottom lines down. Bottom line of 
text is lost. 

ESCM — current line is deleted, lower lines 
scrolled up to close the gap 

ESC P 1 —- turn on printer (Like Ctrl-PrtSc). 
Everything that now appears onscreen 
will be copied to the printer 

USI SES Nik USE printer is put in 
COMPRESSED PRINT MODE. 

ESE F E sae line 51.10%  printer is put in 
EMPHASIZED PRINT MODE. 

ESC P i -- doesn't turn printer on, just sets 
ITALICS mode. So, to print in ITALICS, 
first turn on printer (ESC P 1), then 
have the remote computer send this 

ESC EES turn on printer, and set into 
PROPORTIONAL print mode. 

ESC PO -- turn off printer. Screen is no longer 
copied to printer 

ESC Q سب‎ Enter INSERT mode. All subsequent 

| characters will beg inserted before the 
cursor on this line 

ESC R = Deletes the character at current 
cursor position. 

ESC T A = Cause SIMTERM to begin ADMZA 
simulation | 

ESC T a -- Cause SIMTERM to end ADMSA simulation 

ESC TR -- Cause SIMTERM to set up to play ROGUE, 
so that arrow keys work. 

ESC T r -- Cause SIMTERM to end rogue playing set 
up 

ESC V S -- Cause SIMTERM to set up for vi, so 
that arrows, Ins, Del, PgUp and PgDn do 
reasonable things in this Unix text 
editor 

ESC Vs -- End Vi mode 

ESC [xc ~- causes character ‘c’ to be printed x 
number of times, i.e. to print 27 B's, 
send ESC [ ESC B, since ESC has an ASCII 
value of 27. 

Copyright G 1982,1984 Jim & Eric Holtman 


SIMTERM Instructions Page: 164 


ESC &dB -- set screen attribute to a value that 
equals BOLD on the printer (Actual 
screen color varies from monochrome to 
graphics monitor.) 


ESC &dD -- set attribute to correspond to 
UNDERLINE 

ESC &dH -- set attribute to be SUPERSCRIPT 

ESC &dL -- set attribute to be SUBSCRIPT 


ESC &d@  —— set attribute back to normal 

ESC=y x ~- move cursor to position y,x: where y 
and x are the ASCII characters which 
equal the desired position plus 32. I.e. 
to move to position 3,3 send ESC = # #, 
since the ASCII value of # (35) minus 32 
is 3 

ESC = -—— request ID. SIMTERM will send back the 
string "IBM PC" terminated by a newline. 


10. XMODEM Under UNIX 


When using XMODEM with a UNIX system, the programs 
"xrecv.C and 'xtrans.c' should be uploaded and compiled on 
the host. These programs are setup for the.System V release 
of UNIX and may need rework if used on other systems (e.g., 
Berkeley). To send a file from the PC->UNIX, type "xrecy 
filename’ on UNIX and when the prompt comes up to start 
transmission, hit Alt-T and follow the instructions. 


To send a file from UNIX->PC, type "xtrans 
filename’ on UNIX and when the prompt comes up to set up the 
receive, hit Alt-R and follow the instructions. 


If you are sending ASCII Text files, invoke the 


"xrecv / xtrans with the '-t' argument so that conversion 
is done between the DOS and UNIX conventions. The XMODEM 
protocol using the '-t' option is the safest way to transfer 


text files between systems since checks are made for 
successful transmission and any errors introduced by noisy 
lines will be filtered out. 

Notes Lt the ‘-d’ option is given to either 
program, the following will occur. ×۴ ح۷٤‎ leaves a 
running commentary on the connection status in "xr.err", 
and xtrans leaves a corresponding file called "xt.err". They 
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are used for debugging purposes. 
11. Using SIMTERM with Other Modems 


SIMTERM has been tailored to work with the VENTEL 
or Hayes modem. If you are using it with another modem then 
make sure that the modem is optioned to present the ‘modem 
ready’ signal. If you want to be able to ‘dial’ the phone 
number from the keyboard, you will also have to option it to 
present the ‘carrier detect’ signal. SIMTERM will not get 
past its initialization code unless it see both the carrier 
detect and the modem ready signals. 


If you are using an acoustic coupler or a 212 
“dial up’, then when the message ‘establish communications’ 
appears, dial up the computer and enable the ‘data’ (é.g., 
hit the DATA button on the 212 or put the receiver in the 
coupler). 


If you are using any modem (or are directly 
connected), and SIMTERM seems to hang right after the 
“Establish communications link" message, it probably means 
that your modem (or local connection?) is not outputting the 
correct RS-232. signals for SIMTERM. If the equipment's not 
yours, or you don't understand how to fix the problem, or 
you're just too lazy to bother, try invoking SIMTERM with 
the -d flag. I.e. say "“simterm -d" instead of just 
"simterm". This will keep SIMTERM from looking at the state 
Of your modem, and your problem will disappear. If it still 
doesn't work, you can always give us a call. | 


Dos Compiling the Simulator 


If you look at the link control file 'simterm.lnk' 
you will see the programs that have to be compiled or 
assembled. You must use the MACRO assembler (masm). The 
file ‘struct.mac’ has some macros for structured assembly 
code which makes it easier to write. The file 'mactest.asm' 
has some examples of the use of the structured macros. The 
'relations' between variable or constants are the same as 
the conditional jumps with the 'j' removed. 'long' can be 
appended to the macro to cause a long jump to be generated. 


'com.asm' is the interface to the Async Comm line. 
The file 'comm.inc' has the entry points defined. 
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The file `comm. inc ' has the entry points defined. 
'xxgraph.asm' has functions for cursor control and such from 
PASCAL programs. The file 'graph.inc' has the entry points 
defined. 


"'arglist.obj' and 'arglist.inc' provide ‘argv’ and 
‘argc’ for the parameters to the PASCAL program. See 
'fainsim.pas' for examples of their use. Most of the program 
will be self-explanatory (hopefully). Any questions, give me 
a call. 


If you want to run SIMTERM on a PC with 128K, you 
will have to have access to the compiler and macro 
assembler. Instead of re-compiling and linking the programs 
in  simterm.lnk, use "smterm.lnk'". This will build a SIMTERM 
without  VenTel features and without Login Scripts, but it 
also knocks about SOK off the '.exe' file. 


E Funny Error Conditions 


Sometimes  SIMTERM will appear to lockup and not 
accept any input from the keyboard (you will hear the bell 
indicating that the input buffer is full). This is usually 
Caused. by an incomplete ESCAPE sequence that has been sent 
to SIMTERM and it 1s waiting for the rest of the characters, 
which will probably never come from the computer. To ‘break 
out’ of this condition, depress at the same time the ‘Ctrl’, 
‘Shift’, and ‘Alt’ keys on the left side of the keyboard. 
This will cause SIMTERM to terminate its ‘wait state’ and 
continue. This sometimes happens after dialing into a BBS or 
other system since you might get some noise of the line. 


also the transmitter may have missed an interrupt, 
and what you are typing is not sent. If you suspect this, 
Nit Altf. 


If SIMTERM gets into a funny condition that you 
can not get it out of, then terminate it with 'Alt-F1' and 
then reinvoke it. This procedure will leave the PC attached 
to the system and you can pick up where you left off. 


There have been problems on some systems that use 


PROKEY,  NEWKEY, or other software that interprets the 
keyboard. If SIMTERM is locking up on you and you Rave a 
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program like this loaded, reboot the system without it and 
try it. Whatever those programs are doing, they are not very 
friendly to SIMTERM. 


Jim Holtman Eric Holtman 

35 Dogwood Trail Watson Dorm, Room 142 
Randolph, N. J. 07869 Station #2 

UNI) —-sS61—55793 Charlottesville,VA 22904-0029 
Net Address: ...!'harpo!whuxlb!jph ...'harpo'whuxle!eric 


If you add any interesting new features to the 
program, please keep me informed. 


I have several other public domain programs that I 
will be willing to trade for ones that I don't have. Drop 
me a note (or a floppy disk) with what you have and we can 
setup a trade. Changes and improvements are continually 
being added to the program and if you are interested in what 
the changes are, just drop me a note (or a floppy) for the 
latest information. 
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APPENDIX J 
ASSEMBLY LANGUAGE ROUTINES - IBM-PC 
This Appendix contains the listings of the file 
"biosio.asm" and "“dostime.asm", used in the IBM-PC Kalman 
Filter program to generated graphic output and to evaluate 
execution time respectively. The "biosio.asm" is also used 
by the plotting routine, PLOT.C . The files "biosio.asm" and 
"dostime.asm" are part of the Public Domain Library and can 


be found in the PC-SIG library disk number 216. 


e we We we a8 WS 20 WR WS 08 00 MER ad Ja as 


MICROSOFT 
interface directly with the basic I/O system (BIOS). 


%£%# biosio.asm *** 


IBM-PC microsoft "C" under PC-DOS v2.00 


"C" callable 8088 assembly routines that 


NOTE —- The IBM Technical Reference Manual contains a 
listing of the BIOS and more complete descriptions of each 
interrupt. 


Written by L. Cuthbertson, April 1984 
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3 
PGROUP GROUP PROG 
PROG SEGMENT BYTE PUBLIC 'PROG' 


PUBLIC 
PUBLIC 
PUBLIC 
PUBLIC 
PUBLIC 


ASSUME 


synopsis 


BIOSINI,BIOSSET,BIOSPOS 

BIOSUP ,BIOSDWN, BIOSRCA, BIOSWCA, BIOSWC 
BIOSWD ,BIOSTTY,BIOSCUR 

COMINI ,COMOUT , COMIN, COMSTAT 

INP ,OUTP 


CS: PGROUP 
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***x VIDEO 1/0 x** 


NOTE = the video I/O interrupt description starts on page 
A-43 of the Tech Ref Manual. 
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Initialize screen I/O using the BIOS set mode call 


biosini (stype); 


int stype; screen type 


40x25 BW (power on default) 
40x25 Color 

80x25 BW 

80x25 Color 

raphics mode 

220x200 Color 

320x200 BW 

640x200 BW 


© O (۳ نا‎ UNO 
H H H Il 


ll H ۷ 


s internal use only 

: 7 = 80x25 BW card 

3 

BIOSINI PROC NEAR 
PUSH BP 
MOV BP,SP 
MOV AL, [BP +4] s SCREEN TYPE IN AL 
MOV AH.O ; SET MODE FUNCTION 
INT 10H s BIOS VIDEO 1/0 INTERRUPT 
POP BP 
RET 

BIOSINI ENDP 

3 

; Set the current cursor position. 

3 

s; synopsis  biosset(irow,icol); 

3 

; *33« no value returned d 

; int irow; O to 24 

s int icol; 0 to 72 

3 

BIOSSET PROC NEAR 
PUSH BP 
۲٢۷ BP ,SP 
MOV DH,LBP-4] s ROW 
MOV DL,CBP+6) 5 COLUMN 
MOV BH,O CURRENT PAGE NUMBER 
MOV AH,2 CURSOR POSITION SET FUNTION NUMBER 
INT 10H s VIDEO I/O INTERRUPT 
POP BP 
RET 

BIOSSET ENDP 


IOSPOS 


PUSH 


MOV 
MOV 
MOV 
INT 
MOV 
MOV 
POP 
RET 
65 


Return the current cursor postion. 


synopsis iret = biospos(); 


int iret; high order bits contain row 
low order bits contain column 


PROC NEAR 
BP 
BP, SP 
BH,O ; CURRENT PAGE NUMBER 
AH,2 ; CURSOR POSITION FUNCTION NUMBER 
10H ; VIDEO I/O INTERRUPT 
^H , DH ; MOVE INT RETURN INTO FUNCTION RETURN 
AL ,DL : DITTO 
BP 
ENDP 


«AB AB WB we we SAG ليده‎ © “ME 


s manual 


5 
BIOSRCA 


PUSH 


MOV 
MOV 
MOV 
INT 
POP 
RET 
BIOSRCA 


Write 


5 
3 
$ 
3 
3 
3 
5 
3 
5 
5 
3 
s manual 
s 
B 


1006 


PUSH 


MOV 
MOV 
MOV 
MOV 
MOV 
MOV 
INT 
EP 
RET 
BIOSWCA 


Write 


ee “we Wa we Ce 


Read the contents of a given screen cell. 


synopsis iretsbiosrca()s 


int iret; high order bits contain attributes 
low order bits contain character 


NOTE - Attributes are defined on page 13-9 of the DOS v2.0 


PROC NEAR 
BP 
BP , SP 
BH,O : ACTIVE DISPLAY PAGE 
۵۲۵ : READ CHARACTER + ATTRIBUTES FUNCTION 
10H ; VIDEO I/0 INTERRUPT 
BP 
ENDP 


a character to the screen - with attributes. 


synopsis  bioswca(char,count,att); 


«3x34 no value returned ***x 


int char; character to output 
int count; number of times to output character 
int att; character attribute 


NOTE - Attributes are defined on page 13-9 of the DOS v2.0 


PROC NEAR 


BP 
EPS SP 
AL, [BP+41 s CHARACTER 
CA اد‎ ; NUMBER OF CHARACTERS TO WRITE 
BL,CBP+8] s CHARACTER ATTRIBUTE 
BH, O s ACTIVE DISPLAY PAGE 
AH,9 s WRITE CHARACTER/w ATTRIBUTES FUNCTION 
10H s VIDEO I/O INTERRUPT 
BP 
ENDP 
a character to the screen - no attributes. 


synopsis  bioswc(char,count); 
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X343 no value returned *** 


5 
Š int char; character to output 
: int count; number of times to output character 
9 
BIOSWC PROC NEAR 
PUSH BP 
MOV BP,SP 
MOV AL,CBP+4] s CHARACTER 
MOV CX,LBP+61 s NUMBER OF CHARACTERS TO WRITE 
MOV BL,O; s CHARACTER ATTRIBUTE - NULL 
MOV BH,O s ACTIVE DISPLAY PAGE 
MOV AH. 10 s WRITE CHARACTER ONLY FUNCTION NUMBER 
INT 10H s VIDEO I/O INTERRUPT 
POP BP 
RET 
BIOSWC ENDP 
9 
; Write a dot in graphics mode. 
3 
: synopsis  bioswdí(irow,icol); 
5 
; *34* no value returned *#* 
; int irow; 
: int icol; 
5 
BIOSWD PROC NEAR 
PUSH BP 
MOV BP, SP 
MOV 12 ۳+4ظ۲ ] و‎ [ ; ROW 
MOV CX, CBP+6] s COLUMN 
MOV AL,1 s GREEN COLOR 
MOV AH, 12 s WRITE A DOT FUNCTION NUMBER 
INT 10H s VIDEO I/O INTERRUPT 
. POP BP 
RET 
BIOSWD ENDP 
5 
9 
s Return the current video state of the screen. 
3 
s synopsis iret = bioscur(); 
s 
s int iret; low bits are the mode currently set 
; (see biosini for description) 
: high bits are the number of columns on 
screen 
9 
BIOSCUR PROC NEAR 
PUSH BP 
MOV BP ,SP 
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MOV 
INT 
POP 
RET 
BIOSCUR 


7 


000 
001 
010 
011 
100 
101 
110 
111 


1 ( ۶ وده‎ we We ههه‎ We ههه‎ We We we We we Wa وها وها ويه مها وها وها ويه‎ UL US وه وها وهه‎ “US Ul “M06 Us a 


OMINI 


PUSH 


MOV 
MOV 
MOV 
MOV 
INT 
POP 
RET 
COMINI 


AH,15 
10H s BI 
BP 


ENDP 


*** comm 


synopsis iret = c 


int iret; 
int port; 
int params; 


6 5 4 


-BAUD RATE- 


e Re 
= 0 
- 300 
- 6800 
= 1200 
- 2400 
- 4800 
- 9600 


PROC NEAR 
BP 

BP,SP 

DX, CBP+4] 

AL, CBP+4] 

AH, O 

14H ; CO 

BP 


ENDP 


synopsis iret =c 


s CURRENT VIDEO STATE FUNCTION 
OS VIDEO 1/0 INTERRUPT 


HH ہد ہہ‎ HE HE HE EH EE EEE EEE EEE EE ETE EHH 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 


unications port s* 


NOTE - the communications port I/O is described starting 
on page 4-20 of the Tech Ref Manual. 


Jt JC JC JE JC JE JC 9€ JC 9€ 9€ 9€ 9€ JC 9e 969€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 96 9€ 9€ 9€ 9€ 96 9€ 9€ 9€ 9 9€ 9€ 9€ 9€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 96 9€ 9€ 9€ 9€ 39€ 9€ 


Initialize the communications port. 


omini (port,params); 


return status (see comstat) 
communications port to initialize (0,1) 
bit pattern for initialization - 


3 2 1 O 
-PARITY- -STOP BIT- -WORD LENGTH- 


XO - NONE O = 1 10 — 7 BITS 
OT CDP S IAS BS BITS 
11 — EVEN 


; COMM PORT TO INITIALIZE 
; PARAMETERS 

s INITIALIZATION FUNCTION 
MM PORT I/O INTERRUPT 


Write a character to the communications port. 


omout (port,c); 


(C) ۰ aoe we we we 


int iret; error return ( 5127 if error occured) 
— see AH under modem control 


int Por" communications port to write (0 or 1) 
int e; character to write. 
OMOUT PROC NEAR 
PUSH BP 
MOV BP,SP 


MOV DX,CBP+4] ; COMM PORT TO WRITE 

MOV AL,LBP-*63J ; CHARACTER TO WRITE 

MOV AH,1 s WRITE CHARACTER FUNCTION 
INT 14H 3; COMM PORT I/O INTERRUPT 

POP BP 

RET 


COMOUT ENDP 


C) ° 26 ‘Sn "aA وهه‎ "049 We Ag Cb we We we ےھ‎ 


Read a character from the communications port. 
Waits for character if one is not ready. 

See \comm\ibmtty.c for example of polling comm port 
for character without walt. 


synopsis c = comin(port); 
جع ات7‎ character read from comm port, 


^ 127 if error or no character ready. 
— see AH under modem control 


int port; communications port to read (O or 1) 

DOMIN PROC NEAR 

PUSH BP 

۲٢۷ ,6ع‎ ۴ 

MOV DX,CBP+4] ; COMM PORT TO READ 

MOV AH,2 s READ CHARACTER FUNCTION 

INT 14H s COMM PORT I/O INTERRUPT 

FUP TBP 

RET 


COMIN ENDP 


ae we 0 "0 "09 a0 WS ‘ee Ws 204 00 "aS “28 


Check the line and modem status 


synopsis iret = comstat (port); 
int iret; s line and modem status 
AH - high order bits contain line control status 
bit 7 = time out 
bit á = trans shift register empty 
bit 5 = trans holding register empty 
bit 4 = break detect 
bit 3 = framing error 
bit 2 = parity error 


2358 


(C) ws "AB "858 we AB Sa 0 0 “88 ‘8 وهه‎ 8 "A58 ‘ee ^88 *AB ^85 


OMSTAT 


PUSH 


MOV 
MOV 
MOV 
INT 
POP 
RET 
COMSTAT 


@ we 88 وهه‎ 00 ea We We 8 هه 88 وهه‎ 


bit 1 = overrun error 
bit O = data ready 


AL - low order bits contain modem status 


bit 7 = received line signal detect 

bit 6 = ring indicator 

bit Š = data set ready 

bit 4 = clear to send 

bit 3 = delta receive line signal detect 
bit 2 = trailing edge ring detector 

bit 1 = delta data set ready 

bit O > delta clear to send 


Note — from page A-21 of Tech Ref Manual 


ine port: s communications port to check 
PROC NEAR 
BP 
PP, SP | 
DX,LBP-*4] s COMM PORT TO CHECK 
AH, 3 s STATUS FUNCTION 
14H . s COMM PORT 1/0 INTERRUPT 
BP 
ENDP 
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XXX 10.asm Xxx 


IBM-PC 8088 assembly for interface with microsoft "C" 
under PC-DOS 


"C" callable subroutines that provide absolute pointer 
addressing. 

Pointers within a IBM-PC microsoft "C" program are 
srelative to the start of the programs data segment. These 


ssubroutines provide a mechanism to address absolute memory 
slocations. 


1984 


we We We Wwe we we We we wea‏ دم 


Supplied by microsoft — commented by L. Cuthbertson, April 


3 JE JE 3E JEJE 36 9€ 3€ 3e Je JE 3E JE 3e JE 3E JC IEEE OC UE JE E HE د‎ 3: 3: 3 AE جر نہ جر جرج ہی جر‎ JE 3 X E د‎ E A E E EO EE e oro de ob خب‎ 
Read an absolute memory location. 


synopsis c = readabs(loc); 


5 1 ۲: 5 contents of memory location 
s int loc; absolute memory location in hex 


3 
INP PROC NEAR 
PUSH BP 
MOV BP,SP 
MOV DX,CBP+4]  ;GET LOCATION ADDRESS 
IN AL,DX ; READ LOCATION 
XOR AH,AH ; CLEAR HIGH BYTE 
POP BP 
RET 
INP ENDP 


Write an absolute memory location. 
synopsis  writeabs(loc,c); 


s 
3 
3 
S 
3 
s Int loc; absolute memory location in hex 
5 
-] 
Q 


int cj integer to write to memory location 
UTP PROC NEAR 
PUSH BP 
MOV BP,SP 


MOV DX, LBP +4] ; GET LOCATION ADDRESS 
MOV AX, CBP+6) ;GET ADDRESS OF INTEGER TO WRITE 
OUT DX,AL sWRITE TO MEMORY LOCATION 
POP BP 
RET 
OUTP ENDP 


9 
gE E E E E E E E ہی نہیں نہیں‎ KE KE JE ہی‎ E E E E ہی ہی ہی ہی ہر‎ E E E KE K KE E E E E E E لب‎ E E AE لد ہد بد ہد ہبہ‎ OE JE لد لد‎ FF لد‎ E E E جد‎ 


3 
PROG ENDS 
END 


3 

3 3+ + + dostime.asm 9 J3|w* 

3 

s IBM-PC microsoft "c" under PC-DOS 

1 

s Microsoft "C" callable 8088 assembly routine that uses a 
sDOS function call to obtain and return the hours, minutes, 
;seconds, and hundreths of a second as integers. 

3 


synopsis dostime(*#hour,%*min,%sec,%wths); 


2 3 + no function value returned %#%#%+ 


G Aa We We We We ua 


int hour; 0-23 military time 
int min; 0—99 
int sec; 0-59 
int ths; 0-69 


Written by L. Cuthbertson, May 1984 


Sas “Ae للد‎ "AS 


Jt 3€ 3t 3€ 3t 9€ 9 J€ J€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 3€ 3€ 9€ 9€ 9€ 9€ 9€ 3€ 9€ 9€ 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9E 9€ 9E 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 


3 

PGROUP GROUP PROG 

PROG SEGMENT BYTE PUBLIC ‘PROG’ 
PUBLIC DOSTIME 
ASSUME CS: PGROUP 


HH HH HH EE EE EE EE EE EE EE EE EEE EE EE EEE 9€ 9€ 3€ 3€ 9E E 9€ 3€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 9€ 


5 
š 
3 
D 


OSTIME PROC NEAR 
PUSH BP 
MOV BP,SP 
MOV AH,2CH DOS GET TIME FUNCTION NUMBER 
INT 21H ; DOS FUNCTION CALL INTERRUPT 
MOV AL,CH MOVE HOURS INTO AX 
XOR AH,AH CLEAR HIGH BYTE 


MOV DI,CBP+4] 
MOV CDI1,AX 
MOV BL,CL 
XOR BH,BH 
MOV DI,CBP+4] 
MOV EDIJ,BX 
MOV CL,DH 
XOR CH,CH 
MOV DI,CBP+8] 
MOV EDIJ,CX 
XOR DH,DH 
MOV DI,CBP+10] 
MOV CDI1,DX 
POP BP 
RET 

DOSTIME ENDP 


ADDRESS OF HOURS VARIABLE 

MOVE HOURS INTO HOURS VARIABLE 
MOVE MINUTES INTO BX 

CLEAR HIGH BYTE 

ADDRESS OF MINUTES VARIABLE 

MOVE MINUTES INTO MINUTES VARIABLE 
MOVE SECONDS INTO CX 

CLEAR HIGH BYTE 

ADDRESS OF SECONDS VARIABLE 

MOVE SECONDS INTO SECONDS VARIABLE 
CLEAR HIGH BYTE 

ADDRESS OF THS VARIABLE 

MOVE 1/100THS INTO THS VARIABLE 


2s We was ws We ABS ws We ABS 0 AB IM IGI BBS U) = 


3 
PROG ENDS 
END 
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